# read/write stuff on screen std = None # Robot data t = Twist() pose = Pose() ekfOdom = Odometry() radStep = deg2rad(15) transListener = None #laser information laserInfo = LaserScan() #Inertial Unit imuInfo = Imu() #Metal detector data leftCoil = Coil() middleCoil = Coil() rightCoil = Coil() # Camera data size = 800, 800, 3 leftCamInfo = CameraInfo() rightCamInfo = CameraInfo() leftCamFrame = CompressedImage() rightCamFrame = CompressedImage() cv2.namedWindow("Left window", 1) cv2.namedWindow("Right window", 1)
def talker(self, type_of_data): cwd = os.getcwd() gy = []; accel = []; mag = []; timeStamp = []; msg1 = Odometry(); gladiator_files = ['/gyro_gladiator.txt','/mag_gladiator.txt','/accel_gladiator.txt','/timestamp_gladiator.txt'] simluated_files = ['/gyro.txt','/mag.txt','/accel.txt','/timestamp.txt'] files = None; if type_of_data == 1: files = gladiator_files; else: files = simluated_files; print 'type_of_data: ',type_of_data, files f = open(cwd+files[0],'r') reader = csv.DictReader(f, fieldnames=['x','y','z']) for row in reader: quaternion = tf.transformations.quaternion_from_euler(float(row['x'].strip()), float(row['y'].strip()), float(row['z'].strip())); gy.append(quaternion); f.close(); f = open(cwd+files[1],'r') reader = csv.DictReader(f, fieldnames=['x','y','z']) for row in reader: mag.append([float(row['x'].strip()), float(row['y'].strip()), float(row['z'].strip())]); f.close(); f = open(cwd+files[2],'r') reader = csv.DictReader(f, fieldnames=['x','y','z']) for row in reader: accel.append([float(row['x'].strip()), float(row['y'].strip()), float(row['z'].strip())]); f.close(); f = open(cwd+files[3],'r') reader = csv.DictReader(f, fieldnames=['x','y']) for row in reader: timeStamp.append(float(row['y'].strip())); f.close(); pub = rospy.Publisher('imu_data', Imu, queue_size=10) sub = rospy.Publisher('vo',Odometry,queue_size=10) rospy.init_node('sender', anonymous=True) rate = rospy.Rate(10) # 10hz print "gy: ",len(gy), "accel: ",len(accel), "mag: ",len(mag), "gy: ",len(timeStamp) i=0; while not rospy.is_shutdown(): imu = Imu(); imu.header.stamp = rospy.Time.now(); msg1.header.stamp = imu.header.stamp; #imu.header.stamp.secs = int(timeStamp[i]); #timestamp.txt #imu.header.stamp.nsecs = int((timeStamp[i] - imu.header.stamp.secs)*pow(10,9)); imu.header.frame_id = 'base_footprint' #gyro.txt #print 'i= ',i,' gy= ',gy[i]; #imu.angular_velocity.x = mag[i][0]; #imu.angular_velocity.y = mag[i][1]; #imu.angular_velocity.z = mag[i][2]; #accel.txt #print ' accel= ',accel[i]; #imu.linear_acceleration.x = accel[i][0]; #imu.linear_acceleration.y = accel[i][1]; #imu.linear_acceleration.z = accel[i][2]; #mag.txt #print ' mag= ',mag[i]; imu.orientation.x = gy[i][0] imu.orientation.y = gy[i][1] imu.orientation.z = gy[i][2] imu.orientation.w = gy[i][3] pub.publish(imu) msg1.header.frame_id = "base_footprint" msg1.pose.pose.position.x = 0 msg1.pose.pose.position.y = 0 msg1.pose.pose.position.z = 0 msg1.pose.pose.orientation.x = 1 msg1.pose.pose.orientation.y = 0 msg1.pose.pose.orientation.z = 0 msg1.pose.pose.orientation.w = 0 msg1.pose.covariance = [0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 99999, 0, 0, 0, 0, 0, 0, 99999, 0, 0, 0, 0, 0, 0, 99999] #rospy.loginfo(imu) sub.publish(msg1) rate.sleep() i = i+1; if i == len(gy)-1: break;
def control(): # sp = np.load("xy_sin.npy") state_sub = rospy.Subscriber("/mavros/state", State, state_cb, queue_size=10) rospy.wait_for_service('mavros/cmd/arming') arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) rospy.wait_for_service('mavros/set_mode') set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode) acutator_control_pub = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=10) local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10) mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose", PoseStamped, queue_size=10) imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, imu_cb, queue_size=10) local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, lp_cb, queue_size=10) local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, lv_cb, queue_size=10) act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub", ActuatorControl, act_cb, queue_size=10) rospy.init_node('control', anonymous=True) rate = rospy.Rate(50.0) # print("*"*80) while not rospy.is_shutdown() and not current_state.connected: rate.sleep() rospy.loginfo(current_state.connected) # print("#"*80) pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 0 offb_set_mode = SetModeRequest() offb_set_mode.custom_mode = "OFFBOARD" arm_cmd = CommandBoolRequest() arm_cmd.value = True last_request = rospy.Time.now() i = 0 act = ActuatorControl() flag1 = False flag2 = False prev_imu_data = Imu() prev_time = rospy.Time.now() prev_omega_x = 0 prev_omega_y = 0 prev_omega_z = 0 prev_vx = 0 prev_vy = 0 prev_vz = 0 delta_t = 0.02 count = 0 theta = 0.0 x_theta = 0.0 y_theta = 0.0 step_size = 0.5 prev_time = 0.0 curr_time = 0.0 # rospy.loginfo("Outside") while not rospy.is_shutdown(): if current_state.mode != "OFFBOARD" and ( rospy.Time.now() - last_request > rospy.Duration(5.0)): offb_set_mode_response = set_mode_client(offb_set_mode) if offb_set_mode_response.mode_sent: rospy.loginfo("Offboard enabled") flag1 = True last_request = rospy.Time.now() else: if current_state.armed == False: arm_cmd_response = arming_client(arm_cmd) if arm_cmd_response.success: rospy.loginfo("Vehicle armed") flag2 = True last_request = rospy.Time.now() # rospy.loginfo("Inside") curr_time = rospy.Time.now() if flag1 and flag2: x_f.append(float(local_position.pose.position.x)) y_f.append(float(local_position.pose.position.y)) z_f.append(float(local_position.pose.position.z)) vx_f.append(float(local_velocity.twist.linear.x)) vy_f.append(float(local_velocity.twist.linear.y)) vz_f.append(float(local_velocity.twist.linear.z)) # print(local_velocity.twist.linear.x) orientation = [ imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z ] (roll, pitch, yaw) = quaternion_to_euler_angle( imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z) r_f.append(float(mt.radians(roll))) p_f.append(float(mt.radians(pitch))) yaw_f.append(float(mt.radians(yaw))) sin_r_f.append(mt.sin(float(mt.radians(roll)))) sin_p_f.append(mt.sin(float(mt.radians(pitch)))) sin_y_f.append(mt.sin(float(mt.radians(yaw)))) cos_r_f.append(mt.cos(float(mt.radians(roll)))) cos_p_f.append(mt.cos(float(mt.radians(pitch)))) cos_y_f.append(mt.cos(float(mt.radians(yaw)))) rs_f.append(float(imu_data.angular_velocity.x)) ps_f.append(float(imu_data.angular_velocity.y)) ys_f.append(float(imu_data.angular_velocity.z)) ix.append(float(ixx)) iy.append(float(iyy)) iz.append(float(izz)) m.append(float(mass)) u0.append(act_controls.controls[0]) u1.append(act_controls.controls[1]) u2.append(act_controls.controls[2]) u3.append(act_controls.controls[3]) pose.pose.position.x = -1 * x_theta pose.pose.position.y = x_theta pose.pose.position.z = 3 x_des.append(pose.pose.position.x) y_des.append(pose.pose.position.y) z_des.append(pose.pose.position.z) sin_y_des.append(yaw_des) cos_y_des.append(yaw_des) w_mag.append(0) w_x.append(0) w_y.append(0) w_z.append(0) n_t = curr_time - prev_time #delta_t = float(n_t.nsecs) * 1e-9 a_x.append( (float(local_velocity.twist.linear.x) - prev_vx) / delta_t) a_y.append( (float(local_velocity.twist.linear.y) - prev_vy) / delta_t) a_z.append( (float(local_velocity.twist.linear.z) - prev_vz) / delta_t) ax_f.append(float(imu_data.linear_acceleration.x)) ay_f.append(float(imu_data.linear_acceleration.y)) az_f.append(float(imu_data.linear_acceleration.z)) prev_vx = float(local_velocity.twist.linear.x) prev_vy = float(local_velocity.twist.linear.y) prev_vz = float(local_velocity.twist.linear.z) aplha_x.append( (float(imu_data.angular_velocity.x) - prev_omega_x) / delta_t) aplha_y.append( (float(imu_data.angular_velocity.y) - prev_omega_y) / delta_t) aplha_z.append( (float(imu_data.angular_velocity.z) - prev_omega_z) / delta_t) prev_omega_x = float(imu_data.angular_velocity.x) prev_omega_y = float(imu_data.angular_velocity.y) prev_omega_z = float(imu_data.angular_velocity.z) print('Local Position :', local_position.pose.position.x, ' Des : ', pose.pose.position.x) x_theta = x_theta + 0.01 count += 1 local_pos_pub.publish(pose) if (count >= data_points): break prev_time = curr_time rate.sleep() nn1_neg_x_y_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u3 ], ndmin=2).transpose() nn1_neg_x_y_grd_truth = np.array([a_x, a_y, a_z], ndmin=2).transpose() nn2_neg_x_y_input_state = np.array([ vx_f, vy_f, vz_f, rs_f, ps_f, ys_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, u0, u1, u2 ], ndmin=2).transpose() nn2_neg_x_y_grd_truth = np.array([aplha_x, aplha_y, aplha_z], ndmin=2).transpose() print('nn1_neg_x_y_input_state :', nn1_neg_x_y_input_state.shape) print('nn1_neg_x_y_grd_truth :', nn1_neg_x_y_grd_truth.shape) print('nn2_neg_x_y_input_state :', nn2_neg_x_y_input_state.shape) print('nn2_neg_x_y_grd_truth :', nn2_neg_x_y_grd_truth.shape) np.save('nn1_neg_x_y_input_state.npy', nn1_neg_x_y_input_state) np.save('nn1_neg_x_y_grd_truth.npy', nn1_neg_x_y_grd_truth) np.save('nn2_neg_x_y_input_state.npy', nn2_neg_x_y_input_state) np.save('nn2_neg_x_y_grd_truth.npy', nn2_neg_x_y_grd_truth) s_neg_x_y = np.array([ x_f, y_f, z_f, vx_f, vy_f, vz_f, sin_r_f, sin_p_f, sin_y_f, cos_r_f, cos_p_f, cos_y_f, rs_f, ps_f, ys_f, r_f, p_f, yaw_f ], ndmin=2).transpose() u_neg_x_y = np.array([u0, u1, u2, u3], ndmin=2).transpose() a_neg_x_y = np.array([ax_f, ay_f, az_f], ndmin=2).transpose() print('s_neg_x_y :', s_neg_x_y.shape) print('u_neg_x_y :', u_neg_x_y.shape) print('a_neg_x_y :', a_neg_x_y.shape) np.save('s_neg_x_y.npy', s_neg_x_y) np.save('u_neg_x_y.npy', u_neg_x_y) np.save('a_neg_x_y.npy', a_neg_x_y)
def imu_loop(self, imu): if not rospy.core.is_shutdown(): self.header = imu.header self.orientation = imu.orientation self.angular_velocity = imu.angular_velocity self.linear_acceleration = imu.linear_acceleration self.tf_imu.sendTransform( (self.orientation.x, self.orientation.y, 0), tf.transformations.quaternion_from_euler( 0, 0, self.orientation.z), rospy.Time.now(), "imu", "base_footprint") if __name__ == '__main__': rospy.init_node('imu_covariance') imu_pub = rospy.Publisher("imu_data", Imu, queue_size=1) old_imu = IMU() new_imu = Imu() covariance_matrix = [imu_covariance] * 9 while not rospy.core.is_shutdown(): new_imu.header = old_imu.header new_imu.orientation = old_imu.orientation new_imu.angular_velocity = old_imu.angular_velocity new_imu.linear_acceleration = old_imu.linear_acceleration new_imu.orientation_covariance = covariance_matrix new_imu.angular_velocity_covariance = covariance_matrix new_imu.linear_acceleration_covariance = covariance_matrix imu_pub.publish(new_imu)
def publish_mag(timer_e): mag_msg = Imu() mag_msg.header.frame_id = MAG_FRAME mag_x = read_word_2c() mag_y = read_word_2c() mag_z = read_word_2c()
rospy.init_node('SpartonDigitalCompassIMU') #Pos_pub = rospy.Publisher('AHRS8_HeadingTrue', Pose2D) Imu_pub_q = rospy.Publisher('AHRS8_data_q', Imu) Imu_pub_e = rospy.Publisher('AHRS8_data_e', imu_data) Imu_pub_temp = rospy.Publisher('AHRS8_Temp', Float32) #SpartonPose2D=Pose2D() #SpartonPose2D.x=float(0.0) #SpartonPose2D.y=float(0.0) #Init D_Compass port D_Compassport = rospy.get_param('~port', '/dev/ttyAHRS') D_Compassrate = rospy.get_param('~baud', 115200) # printmodulus set to 1 is 100 Hz. 2 : 50Hz D_Compassprintmodulus = rospy.get_param('~printmodulus', 1) #Digital compass heading offset in degree D_Compass_offset = rospy.get_param('~offset', 0.) Imu_data = Imu() imu_data = imu_data() temp = Float32() Imu_data = Imu(header=rospy.Header(frame_id="AHRS8")) #TODO find a right way to convert imu acceleration/angularvel./orientation accuracy to covariance Imu_data.orientation_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3] Imu_data.angular_velocity_covariance = [1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3] Imu_data.linear_acceleration_covariance = [ 1e-3, 0, 0, 0, 1e-3, 0, 0, 0, 1e-3 ] myStr1 = '\r\n\r\nprinttrigger 0 set drop\r\n' myStr2 = 'printmask gyrop_trigger accelp_trigger or quat_trigger or yawt_trigger or time_trigger or temp_trigger or set drop\r\n'
def navigation_handler(self, data): """ Rebroadcasts navigation data in the following formats: 1) /odom => /base footprint transform (if enabled, as per REP 105) 2) Odometry message, with parent/child IDs as in #1 3) NavSatFix message, for systems which are knowledgeable about GPS stuff 4) IMU messages """ rospy.logdebug("Navigation received") # If we don't have a fix, don't publish anything... if self.nav_status.status == NavSatStatus.STATUS_NO_FIX: return # Changing from NED from the Applanix to ENU in ROS # Roll is still roll, since it's WRT to the x axis of the vehicle # Pitch is -ve since axis goes the other way (+y to right vs left) # Yaw (or heading) in Applanix is clockwise starting with North # In ROS it's counterclockwise startin with East orient = PyKDL.Rotation.RPY(RAD(data.roll), RAD(-data.pitch), RAD(90 - data.heading)).GetQuaternion() # UTM conversion utm_pos = geodesy.utm.fromLatLong(data.latitude, data.longitude) # Initialize starting point if we haven't yet if not self.init and self.zero_start: self.origin.x = utm_pos.easting self.origin.y = utm_pos.northing self.origin.z = data.altitude self.init = True origin_param = { "east": self.origin.x, "north": self.origin.y, "alt": self.origin.z, } rospy.set_param('/gps_origin', origin_param) # Publish origin reference for others to know about p = Pose() p.position.x = self.origin.x p.position.y = self.origin.y p.position.z = self.origin.z self.pub_origin.publish(p) # # Odometry # TODO: Work out these covariances properly from DOP # odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = utm_pos.easting - self.origin.x odom.pose.pose.position.y = utm_pos.northing - self.origin.y odom.pose.pose.position.z = data.altitude - self.origin.z odom.pose.pose.orientation = Quaternion(*orient) odom.pose.covariance = POSE_COVAR # Twist is relative to /reference frame or /vehicle frame and # NED to ENU conversion is needed here too odom.twist.twist.linear.x = data.east_vel odom.twist.twist.linear.y = data.north_vel odom.twist.twist.linear.z = -data.down_vel odom.twist.twist.angular.x = RAD(data.ang_rate_long) odom.twist.twist.angular.y = RAD(-data.ang_rate_trans) odom.twist.twist.angular.z = RAD(-data.ang_rate_down) odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) t_tf = odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z q_tf = Quaternion(*orient).x, Quaternion(*orient).y, Quaternion( *orient).z, Quaternion(*orient).w # # Odometry transform (if required) # if self.publish_tf: self.tf_broadcast.sendTransform(t_tf, q_tf, odom.header.stamp, odom.child_frame_id, odom.header.frame_id) # # NavSatFix # TODO: Work out these covariances properly from DOP # navsat = NavSatFix() navsat.header.stamp = rospy.Time.now() navsat.header.frame_id = self.odom_frame navsat.status = self.nav_status navsat.latitude = data.latitude navsat.longitude = data.longitude navsat.altitude = data.altitude navsat.position_covariance = NAVSAT_COVAR navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.pub_navsatfix.publish(navsat) # # IMU # TODO: Work out these covariances properly # imu = Imu() imu.header.stamp = rospy.Time.now() imu.header.frame_id = self.base_frame # Orientation imu.orientation = Quaternion(*orient) imu.orientation_covariance = IMU_ORIENT_COVAR # Angular rates imu.angular_velocity.x = RAD(data.ang_rate_long) imu.angular_velocity.y = RAD(-data.ang_rate_trans) imu.angular_velocity.z = RAD(-data.ang_rate_down) imu.angular_velocity_covariance = IMU_VEL_COVAR # Linear acceleration imu.linear_acceleration.x = data.long_accel imu.linear_acceleration.y = data.trans_accel imu.linear_acceleration.z = data.down_accel imu.linear_acceleration_covariance = IMU_ACCEL_COVAR self.pub_imu.publish(imu) pass
return {'x': x, 'y': y, 'z': z} def get_all_data(self): """Reads and returns all the available data.""" temp = self.get_temp() accel = self.get_accel_data() gyro = self.get_gyro_data() return [accel, gyro, temp] if __name__ == "__main__": pub = rospy.Publisher('imu', Imu, queue_size=3) rospy.init_node("Imu_data", anonymous=True) rate = rospy.Rate(10) #10hz dados = Imu() while True: mpu = mpu6050(0x68) #mpu.get_temp() dados.header.stamp = rospy.Time.now() accel_data = mpu.get_accel_data() dados.linear_acceleration = Vector3(accel_data['x'], accel_data['y'], accel_data['z']) gyro_data = mpu.get_gyro_data() dados.angular_velocity = Vector3(gyro_data['x'], gyro_data['y'], gyro_data['z']) pub.publish(dados)
rtslam_imu_bag.py <path-to-MTI.log-file> ''' import sys import rospy import rosbag from sensor_msgs.msg import Image, Imu mtilog_path = sys.argv[1] with open(mtilog_path) as f: lines = f.readlines() mti_data = [map(float, line.split()) for line in lines if not line.startswith('#')] imu_msg = Imu() imu_msg.header.seq = 0 with rosbag.Bag("imu.bag", 'w') as bag: for date, acc_x, acc_y, acc_z, angvel_x, angvel_y, angvel_z, mag_x, mag_y, mag_z in mti_data: imu_msg.header.stamp = rospy.Time.from_sec(date) # imu_msg.header.frame_id = ??? imu_msg.linear_acceleration.x = acc_x imu_msg.linear_acceleration.y = acc_y imu_msg.linear_acceleration.z = acc_z imu_msg.angular_velocity.x = angvel_x imu_msg.angular_velocity.y = angvel_y imu_msg.angular_velocity.z = angvel_z # Inform we doesn't have orientation estimates # see http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html imu_msg.orientation_covariance[0] = -1
def spin_once(self): def baroPressureToHeight(value): c1 = 44330.0 c2 = 9.869232667160128300024673081668e-6 c3 = 0.1901975534856 intermediate = 1 - math.pow(c2 * value, c3) height = c1 * intermediate return height # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) #temp = data.get('Temp') # float orient_data = data.get('Orientation Data') velocity_data = data.get('Velocity') position_data = data.get('Latlon') altitude_data = data.get('Altitude') acc_data = data.get('Acceleration') gyr_data = data.get('Angular Velocity') mag_data = data.get('Magnetic') pressure_data = data.get('Pressure') time_data = data.get('Timestamp') gnss_data = data.get('Gnss PVT') status = data.get('Status') # int # create messages and default values "Imu message supported with Modes 1 & 2" imu_msg = Imu() pub_imu = False "SensorSample message supported with Mode 2" ss_msg = sensorSample() pub_ss = False "Mag message supported with Modes 1 & 2" mag_msg = Vector3Stamped() pub_mag = False "Baro in meters" baro_msg = baroSample() pub_baro = False "GNSS message supported only with MTi-G-7xx devices" "Valid only for modes 1 and 2" gnssPvt_msg = gnssSample() pub_gnssPvt = False #gnssSatinfo_msg = GPSStatus() #pub_gnssSatinfo = False # All filter related outputs "Supported in mode 3" ori_msg = orientationEstimate() pub_ori = False "Supported in mode 3 for MTi-G-7xx devices" vel_msg = velocityEstimate() pub_vel = False "Supported in mode 3 for MTi-G-7xx devices" pos_msg = positionEstimate() pub_pos = False secs = 0 nsecs = 0 if time_data: # first getting the sampleTimeFine time = time_data['SampleTimeFine'] secs = 100e-6 * time nsecs = 1e5 * time - 1e9 * math.floor(secs) if acc_data: if 'Delta v.x' in acc_data: # found delta-v's pub_ss = True ss_msg.internal.imu.dv.x = acc_data['Delta v.x'] ss_msg.internal.imu.dv.y = acc_data['Delta v.y'] ss_msg.internal.imu.dv.z = acc_data['Delta v.z'] elif 'accX' in acc_data: # found acceleration pub_imu = True imu_msg.linear_acceleration.x = acc_data['accX'] imu_msg.linear_acceleration.y = acc_data['accY'] imu_msg.linear_acceleration.z = acc_data['accZ'] else: raise MTException( "Unsupported message in XDI_AccelerationGroup.") if gyr_data: if 'Delta q0' in gyr_data: # found delta-q's pub_ss = True ss_msg.internal.imu.dq.w = gyr_data['Delta q0'] ss_msg.internal.imu.dq.x = gyr_data['Delta q1'] ss_msg.internal.imu.dq.y = gyr_data['Delta q2'] ss_msg.internal.imu.dq.z = gyr_data['Delta q3'] elif 'gyrX' in gyr_data: # found rate of turn pub_imu = True imu_msg.angular_velocity.x = gyr_data['gyrX'] imu_msg.angular_velocity.y = gyr_data['gyrY'] imu_msg.angular_velocity.z = gyr_data['gyrZ'] else: raise MTException( "Unsupported message in XDI_AngularVelocityGroup.") if mag_data: # magfield ss_msg.internal.mag.x = mag_msg.vector.x = mag_data['magX'] ss_msg.internal.mag.y = mag_msg.vector.y = mag_data['magY'] ss_msg.internal.mag.z = mag_msg.vector.z = mag_data['magZ'] pub_mag = True if pressure_data: pub_baro = True height = baroPressureToHeight(pressure_data['Pressure']) baro_msg.height = ss_msg.internal.baro.height = height if gnss_data: pub_gnssPvt = True gnssPvt_msg.itow = gnss_data['iTOW'] gnssPvt_msg.fix = gnss_data['fix'] gnssPvt_msg.latitude = gnss_data['lat'] gnssPvt_msg.longitude = gnss_data['lon'] gnssPvt_msg.hEll = gnss_data['hEll'] gnssPvt_msg.hMsl = gnss_data['hMsl'] gnssPvt_msg.vel.x = gnss_data['velE'] gnssPvt_msg.vel.y = gnss_data['velN'] gnssPvt_msg.vel.z = -gnss_data['velD'] gnssPvt_msg.hAcc = gnss_data['horzAcc'] gnssPvt_msg.vAcc = gnss_data['vertAcc'] gnssPvt_msg.sAcc = gnss_data['speedAcc'] gnssPvt_msg.pDop = gnss_data['PDOP'] gnssPvt_msg.hDop = gnss_data['HDOP'] gnssPvt_msg.vDop = gnss_data['VDOP'] gnssPvt_msg.numSat = gnss_data['nSat'] gnssPvt_msg.heading = gnss_data['heading'] gnssPvt_msg.headingAcc = gnss_data['headingAcc'] if orient_data: if 'Q0' in orient_data: pub_imu = True imu_msg.orientation.x = orient_data['Q0'] imu_msg.orientation.y = orient_data['Q1'] imu_msg.orientation.z = orient_data['Q2'] imu_msg.orientation.w = orient_data['Q3'] elif 'Roll' in orient_data: pub_ori = True ori_msg.roll = orient_data['Roll'] ori_msg.pitch = orient_data['Pitch'] ori_msg.yaw = orient_data['Yaw'] else: raise MTException( 'Unsupported message in XDI_OrientationGroup') if velocity_data: pub_vel = True vel_msg.velE = velocity_data['velX'] vel_msg.velN = velocity_data['velY'] vel_msg.velU = velocity_data['velZ'] if position_data: pub_pos = True pos_msg.latitude = position_data['lat'] pos_msg.longitude = position_data['lon'] if altitude_data: pub_pos = True tempData = altitude_data['ellipsoid'] pos_msg.hEll = tempData[0] #if status is not None: # if status & 0b0001: # self.stest_stat.level = DiagnosticStatus.OK # self.stest_stat.message = "Ok" # else: # self.stest_stat.level = DiagnosticStatus.ERROR # self.stest_stat.message = "Failed" # if status & 0b0010: # self.xkf_stat.level = DiagnosticStatus.OK # self.xkf_stat.message = "Valid" # else: # self.xkf_stat.level = DiagnosticStatus.WARN # self.xkf_stat.message = "Invalid" # if status & 0b0100: # self.gps_stat.level = DiagnosticStatus.OK # self.gps_stat.message = "Ok" # else: # self.gps_stat.level = DiagnosticStatus.WARN # self.gps_stat.message = "No fix" # self.diag_msg.header = h # self.diag_pub.publish(self.diag_msg) # publish available information if pub_imu: imu_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time imu_msg.header.stamp.secs = secs imu_msg.header.stamp.nsecs = nsecs self.imu_pub.publish(imu_msg) #if pub_gps: # xgps_msg.header = gps_msg.header = h # self.gps_pub.publish(gps_msg) # self.xgps_pub.publish(xgps_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) #if pub_temp: # self.temp_pub.publish(temp_msg) if pub_ss: ss_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ss_msg.header.stamp.secs = secs ss_msg.header.stamp.nsecs = nsecs self.ss_pub.publish(ss_msg) if pub_baro: baro_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.baro_pub.publish(baro_msg) if pub_gnssPvt: gnssPvt_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time baro_msg.header.stamp.secs = secs baro_msg.header.stamp.nsecs = nsecs self.gnssPvt_pub.publish(gnssPvt_msg) if pub_ori: ori_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time ori_msg.header.stamp.secs = secs ori_msg.header.stamp.nsecs = nsecs self.ori_pub.publish(ori_msg) if pub_vel: vel_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time vel_msg.header.stamp.secs = secs vel_msg.header.stamp.nsecs = nsecs self.vel_pub.publish(vel_msg) if pub_pos: pos_msg.header = h #all time assignments (overwriting ROS time) # Comment the two lines below if you need ROS time pos_msg.header.stamp.secs = secs pos_msg.header.stamp.nsecs = nsecs self.pos_pub.publish(pos_msg)
global pi global handle buf_out = bytearray() buf_out.append(data) try: buf_in = bytearray( pi.i2c_write_i2c_block_data(handle, reg_addr, buf_out)) # print("Writing, wr: ", binascii.hexlify(buf_out), " re: ", binascii.hexlify(buf_in)) except: return False return True imu_data = Imu() # Filtered data imu_raw = Imu() # Raw IMU data #temperature_msg = Temperature() # Temperature #mag_msg = MagneticField() # Magnetometer data # Main function if __name__ == '__main__': rospy.init_node("bosch_imu_node") # Sensor measurements publishers pub_data = rospy.Publisher('imu/data', Imu, queue_size=1) pub_raw = rospy.Publisher('imu/raw', Imu, queue_size=1) # pub_mag = rospy.Publisher('imu/mag', MagneticField, queue_size=1) # pub_temp = rospy.Publisher('imu/temp', Temperature, queue_size=1) # srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
def publishStateRos(tup, ros_pub_dec): # Publish our robot state to ROS topics /robotname/state/* periodically global publish_time, count, impossible_motion publish_time += 1 # print(tup) if bROS: publish_time = 0 # Construct /robotname/state/imu ROS message msg = Imu() roll = tup[2] pitch = tup[3] yaw = tup[4] positions = tup[5:13] #print positions velocities = tup[13:21] #print velocities torques = tup[21:29] # #print torques currents = tup[29:37] # print("Torque : ", max(np.array(torques)), min(np.array(torques))) # print("Currents : ", max(np.array(currents)), min(np.array(currents))) temperatures = tup[37:45] impossible_motion = tup[45] quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) msg.orientation.x = quaternion[0] msg.orientation.y = quaternion[1] msg.orientation.z = quaternion[2] msg.orientation.w = quaternion[3] pubs[0].publish(msg) # Construct /robotname/state/pose msg = Pose() msg.orientation.x = quaternion[0] msg.orientation.y = quaternion[1] msg.orientation.z = quaternion[2] msg.orientation.w = quaternion[3] # TODO: Get height from robot state, have robot calculate it msg.position.z = 0.0 pubs[3].publish(msg) msg = JointState() msg.name = [] msg.position = [] msg.velocity = [] msg.effort = [] for j in range(8): msg.name.append(str(j)) msg.position.append(positions[j]) msg.velocity.append(velocities[j]) msg.effort.append(torques[j]) pubs[1].publish(msg) # Translate for URDF in NGR vision60 = False if(vision60): for i in range(8, 2): msg.position[i] += msg.position[i-1]; msg.velocity[i] += msg.velocity[i-1]; else: # other URDF # for URDF of Minitaur FIXME use the class I put in ethernet.py for RobotType msg.header.seq = count count = count +1 msg.header.stamp = rospy.Time.now() msg.position.extend([0, 0, 0, 0, 0, 0, 0, 0]) msg.velocity.extend([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) msg.effort.extend([0, 0, 0, 0, 0, 0, 0, 0]) msg.name.extend(['8', '9', '10', '11', '12', '13', '14', '15']) msg.position[11], msg.position[10], r = minitaurFKForURDF(msg.position[0], msg.position[1]) msg.position[14], msg.position[15], r = minitaurFKForURDF(msg.position[2], msg.position[3]) msg.position[9], msg.position[8], r = minitaurFKForURDF(msg.position[4], msg.position[5]) msg.position[13], msg.position[12], r = minitaurFKForURDF(msg.position[6], msg.position[7]) # other URDF problems (order important) msg.position[11] = -msg.position[11] msg.position[14] = -msg.position[14] msg.position[9] = -msg.position[9] msg.position[13] = -msg.position[13] pubs[2].publish(msg) msg = Float64MultiArray() msg.data = currents pubs[4].publish(msg) msg = Float64MultiArray() msg.data = temperatures pubs[5].publish(msg) msg = Float64MultiArray() msg.data = [max(abs(np.array(torques))), max(abs(np.array(currents)))] pubs[6].publish(msg) msg = Float64MultiArray() msg.data = [sum(abs(np.array(torques))), sum(abs(np.array(currents)))] pubs[7].publish(msg) msg = Bool() msg.data = impossible_motion pubs[8].publish(msg)
def send(): # init node rospy.init_node('SendIMU',log_level=rospy.DEBUG) rate = rospy.Rate(10) navTransformPub = rospy.Publisher('/gps/fix',NavSatFix,queue_size=100) kalmanPub = rospy.Publisher('/imu/data_raw',Imu,queue_size=100) magnetField = rospy.Publisher('/imu/mag',MagneticField,queue_size=100) lat = 48.893583 lng = 2.194122 # get seal level pressure #seaLevelPressureString = data_fetch(url_byCoord(lat,lng)).get('main').get('pressure') seaLevelPressure = 0.0 # float(seaLevelPressureString) gyroscope_bias_x = 0.000050001 gyroscope_bias_y = 0.001467536 gyroscope_bias_z = 0.002046174 magnetometer_bias_x = 5.67524642536e-06 magnetometer_bias_y = 1.7210987153e-05 magnetometer_bias_z = -6.53303760986e-05 i = 0 while not rospy.is_shutdown(): # get data from serial # ser = serial.Serial('/dev/ttyACM3',115200) # line = ser.readline().strip()[1:] line = "AZERTYUIOPAZERTYUIOAZERTYUIOAZERTYUIOPAZERTY" if len(line) == 48: # Get data sensor floatsArray = array.array('f',line) # Extract pressure localPresureInt32 = array.array('I',line[-4:])[0] localPressureFloatPa = float(localPresureInt32) localPressureFloatHPa = localPressureFloatPa / 100.0 localPressureFloatBar = localPressureFloatHPa / 10.0 #floatsArray[11] = getAltitude( localPressureFloatHPa,float(seaLevelPressure),22.0) #rospy.loginfo('altitude : {0}'.format(floatsArray[11])) #rospy.loginfo('Pa : {0} HPa: {1}'.format(localPressureFloatPa,localPressureFloatHPa)) ################################################## ############### Create msg gps/fix ############### ################################################## # Header gpsMsg = NavSatFix() gpsMsg.header.frame_id = "odom" gpsMsg.header.stamp = rospy.Time.now() # Data gpsMsg.latitude = floatsArray[9] gpsMsg.longitude = floatsArray[10] gpsMsg.altitude = floatsArray[11] # Covariance gpsMsg.position_covariance[0] = 6.5 gpsMsg.position_covariance[4] = 6.5 gpsMsg.position_covariance[8] = 6.5 # Config gpsMsg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Publication navTransformPub.publish(gpsMsg) ################################################### ################## Create msg IMU ################# ################################################### msg = Imu() msg.header.frame_id = "rocket" msg.header.stamp = rospy.Time.now() # NO ORIENTATION : HANDLE BY MAGDWIGHT # Angular_velocity # from degree /s to radian /s msg.angular_velocity.x = floatsArray[3] * 0.0174533 - gyroscope_bias_x msg.angular_velocity.y = floatsArray[4] * 0.0174533 - gyroscope_bias_y msg.angular_velocity.z = floatsArray[5] * 0.0174533 - gyroscope_bias_z # Angular_velocity_covariance # 4000 degree/sec + 16 bits => 0.06103515625 degree => 0.001065264436029053 rad msg.angular_velocity_covariance[0] = 1.135e-6 msg.angular_velocity_covariance[4] = 1.135e-6 msg.angular_velocity_covariance[8] = 1.135e-6 # Linear_acceleration msg.linear_acceleration.x = floatsArray[0] * 9.8 msg.linear_acceleration.y = floatsArray[1] * 9.8 msg.linear_acceleration.z = floatsArray[2] * 9.8 # Linear_acceleration_covariance # 32g + 16 bits => 4.8828125e-4 g => 4.78515625e-3 m/s^2 msg.linear_acceleration_covariance[0] = 2.289772033e-5 msg.linear_acceleration_covariance[4] = 2.289772033e-5 msg.linear_acceleration_covariance[8] = 2.289772033e-5 kalmanPub.publish(msg) #################################################### ################## Magnet message ################## #################################################### magnetMessage = MagneticField() magnetMessage.header.frame_id = "rocket" magnetMessage.header.stamp = rospy.Time.now() magnetMessage.magnetic_field.x = floatsArray[7] * 1e-7 - magnetometer_bias_y magnetMessage.magnetic_field.y = floatsArray[6] * 1e-7 - magnetometer_bias_x magnetMessage.magnetic_field.z = -(floatsArray[8] * 1e-7 - magnetometer_bias_z) magnetMessage.magnetic_field_covariance[0] = 4.66034e-10 magnetMessage.magnetic_field_covariance[4] = 4.66034e-10 magnetMessage.magnetic_field_covariance[8] = 4.66034e-10 magnetField.publish(magnetMessage) #################################################### ################# END MAGNET ##################### #################################################### rate.sleep();
def _create_imu_msg(self): """ Create imu message from ROV data Raises: Exception: No data available """ # Check if data is available if 'ATTITUDE' not in self.get_data(): raise Exception('no ATTITUDE data') #TODO: move all msgs creating to msg msg = Imu() self._create_header(msg) #http://mavlink.org/messages/common#SCALED_IMU imu_data = None for i in ['', '2', '3']: try: imu_data = self.get_data()['SCALED_IMU{}'.format(i)] break except Exception as e: pass if imu_data is None: raise Exception('no SCALED_IMUX data') acc_data = [imu_data['{}acc'.format(i)] for i in ['x', 'y', 'z']] gyr_data = [imu_data['{}gyro'.format(i)] for i in ['x', 'y', 'z']] mag_data = [imu_data['{}mag'.format(i)] for i in ['x', 'y', 'z']] #http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html msg.linear_acceleration.x = acc_data[0] / 100 msg.linear_acceleration.y = acc_data[1] / 100 msg.linear_acceleration.z = acc_data[2] / 100 msg.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] msg.angular_velocity.x = gyr_data[0] / 1000 msg.angular_velocity.y = gyr_data[1] / 1000 msg.angular_velocity.z = gyr_data[2] / 1000 msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] #http://mavlink.org/messages/common#ATTITUDE attitude_data = self.get_data()['ATTITUDE'] orientation = [attitude_data[i] for i in ['roll', 'pitch', 'yaw']] #https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_Angles_to_Quaternion_Conversion cy = math.cos(orientation[2] * 0.5) sy = math.sin(orientation[2] * 0.5) cr = math.cos(orientation[0] * 0.5) sr = math.sin(orientation[0] * 0.5) cp = math.cos(orientation[1] * 0.5) sp = math.sin(orientation[1] * 0.5) msg.orientation.w = cy * cr * cp + sy * sr * sp msg.orientation.x = cy * sr * cp - sy * cr * sp msg.orientation.y = cy * cr * sp + sy * sr * cp msg.orientation.z = sy * cr * cp - cy * sr * sp msg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0] self.pub.set_data('/imu/data', msg)
#!/usr/bin/env python import rospy from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry from rtabmap_ros.srv import * import tf rospy.init_node('vo_restarter.py', anonymous=True) last_odom = Odometry() last_imu = Imu() def reset_odom_to(p, o): rospy.logdebug('Restarting Odometry') rospy.wait_for_service('reset_odom_to_pose') try: reset = rospy.ServiceProxy('reset_odom_to_pose', ResetPose) eu = tf.transformations.euler_from_quaternion( (p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w)) reset(o.pose.pose.x, o.pose.pose.y, o.pose.pose.z, eu[0], eu[1], eu[2]) rospy.logdebug('Service reset successfull') except rospy.ServiceException, e: rospy.logfatal('Service call to restore vo failed') def store_last_odom(data): global last_imu, last_odom if data.pose.covariance[0] == 9999.0:
#!/usr/bin/env python import rospy import time import threading from MinIMU_v5_pi import MinIMU_v5_pi from sensor_msgs.msg import Imu from sensor_msgs.msg import MagneticField from std_msgs.msg import String #from geometry_msgs.msg import Vector3Stamped imuData = Imu() magData = MagneticField() #magData=Vector3Stamped() pub_Imu = rospy.Publisher('imu/data_raw', Imu, queue_size=10) pub_Mag = rospy.Publisher('imu/mag', MagneticField, queue_size=10) magx_max = 0.5308837890625 magx_min = -0.0865478515625 magy_max = -0.2818603515625 magy_min = -0.8355712890625 magz_max = 0.8919677734375 magz_min = 0.3597412109375 #pub_Strx = rospy.Publisher('stringx', String, queue_size=10) #pub_Stry = rospy.Publisher('stringy', String, queue_size=10) #pub_Strz = rospy.Publisher('stringz', String, queue_size=10) #pub_vector3= rospy.Publisher('imu/mag', Vector3Stamped, queue_size=10) cnt = 0
#Digital compass heading offset in degree # D_Compass_offset = rospy.get_param('~offset',0.) D_Compass_declination = rospy.get_param('/declination',-7.462777777777778)* (math.pi/180.0) # By defaule IMU use Megnatic North as zero degree in Quaternion # If we want to use it directly with GPS-UTM x,y as Global Heading, East is our zero D_Compass_UseEastAsZero = rospy.get_param('/UseEastAsZero',True) rospy.loginfo("Declination: " + str(D_Compass_declination)) rospy.loginfo("UserEastAsZero: " + str(D_Compass_UseEastAsZero)) # use this try to control miss sync in USB-serial, when it happends, must restart Checksum_error_limits =rospy.get_param('~Checksum_error_limits', 10.) checksum_error_counter=0 imu_data = Imu() imu_data = Imu(header=rospy.Header(frame_id="SpartonCompassIMU")) #TODO find a right way to convert imu acceleration/angularvel./orientation accuracy to covariance imu_data.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] imu_data.angular_velocity_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] imu_data.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6] myStr1='\r\n\r\nprinttrigger 0 set drop\r\n'
def __init__(self): rospy.init_node('brick_node', anonymous=True) rospy.loginfo("Start Initializing IMU") HOST = "localhost" PORT = 4223 UID = "6dJCzE" # Change XXYYZZ to the UID of your IMU Brick 2.0 self.deg_to_rad = np.pi / 180.0 self.accx_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.accy_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.accz_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.eulx_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.euly_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.eulz_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.radx_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.rady_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.radz_list = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] self.q0 = 0.0 self.q1 = 0.0 self.q2 = 0.0 self.q3 = 0.0 self.x_rad = 0.0 self.y_rad = 0.0 self.z_rad = 0.0 self.x_acc = 0.0 self.y_acc = 0.0 self.z_acc = 0.0 self.covariance_x_eul = 0.0 self.covariance_y_eul = 0.0 self.covariance_z_eul = 0.0 self.covariance_x_acc = 0.0 self.covariance_y_acc = 0.0 self.covariance_z_acc = 0.0 self.covariance_x_rad = 0.0 self.covariance_y_rad = 0.0 self.covariance_z_rad = 0.0 ipcon = IPConnection() # Create IP connection self.imu = BrickIMUV2(UID, ipcon) ipcon.connect(HOST, PORT) # Connect to brickd self.pub = rospy.Publisher('imu', Imu, queue_size=5) self.brick_pub = Imu() self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.cb_quaternion) self.imu.register_callback(self.imu.CALLBACK_ORIENTATION, self.cb_euler) self.imu.register_callback(self.imu.CALLBACK_LINEAR_ACCELERATION, self.cb_linacc) self.imu.register_callback(self.imu.CALLBACK_ANGULAR_VELOCITY, self.cb_angvel) self.imu.set_quaternion_period(5) self.imu.set_orientation_period(5) self.imu.set_linear_acceleration_period(5) self.imu.set_angular_velocity_period(5) self.rate = rospy.Rate(50) rospy.loginfo("Initialization done") while not rospy.is_shutdown(): self.publish() self.rate.sleep() if rospy.is_shutdown(): rospy.loginfo("Shutting down brick node") ipcon.disconnect() rospy.signal_shutdown('brick_node')
def main(): # init imu node rospy.init_node('imu', anonymous=False) # get imu config imu_manager = imu.IMU(rospy.get_param('/ema_fes_cycling/imu')) # list published topics pub = {} for name in imu_manager.imus: pub[name] = rospy.Publisher('imu/' + name, Imu, queue_size=10) pub[name + '_buttons'] = rospy.Publisher('imu/' + name + '_buttons', Int8, queue_size=10) # define loop rate (in hz) rate = rospy.Rate(200) # node loop while not rospy.is_shutdown(): try: timestamp = rospy.Time.now() frame_id = 'base_link' if imu_manager.streaming == False: ## messages are shared by all imus imuMsg = Imu() imuMsg.header.stamp = timestamp imuMsg.header.frame_id = frame_id buttons = Int8() for name in imu_manager.imus: orientation = imu_manager.getQuaternion(name) imuMsg.orientation.x = orientation[0] imuMsg.orientation.y = orientation[1] imuMsg.orientation.z = orientation[2] imuMsg.orientation.w = orientation[3] angular_velocity = imu_manager.getGyroData(name) imuMsg.angular_velocity.x = angular_velocity[0] imuMsg.angular_velocity.y = angular_velocity[1] imuMsg.angular_velocity.z = angular_velocity[2] linear_acceleration = imu_manager.getAccelData(name) imuMsg.linear_acceleration.x = -linear_acceleration[0] imuMsg.linear_acceleration.y = -linear_acceleration[1] imuMsg.linear_acceleration.z = -linear_acceleration[2] pub[name].publish(imuMsg) buttons = imu_manager.getButtonState(name) pub[name + '_buttons'].publish(buttons) else: if imu_manager.broadcast == False: for name in imu_manager.imus: ## one message per imu imuMsg = Imu() imuMsg.header.stamp = timestamp imuMsg.header.frame_id = frame_id buttons = Int8() streaming_data = imu_manager.getStreamingData(name) idx = 0 for slot in imu_manager.streaming_slots[name]: #print name, slot if slot == 'getTaredOrientationAsQuaternion': imuMsg.orientation.x = streaming_data[idx] imuMsg.orientation.y = streaming_data[idx + 1] imuMsg.orientation.z = streaming_data[idx + 2] imuMsg.orientation.w = streaming_data[idx + 3] idx = idx + 4 elif slot == 'getNormalizedGyroRate': imuMsg.angular_velocity.x = streaming_data[idx] imuMsg.angular_velocity.y = streaming_data[idx + 1] imuMsg.angular_velocity.z = streaming_data[idx + 2] idx = idx + 3 elif slot == 'getCorrectedAccelerometerVector': imuMsg.linear_acceleration.x = -streaming_data[ idx] imuMsg.linear_acceleration.y = -streaming_data[ idx + 1] imuMsg.linear_acceleration.z = -streaming_data[ idx + 2] idx = idx + 3 print type(streaming_data) elif slot == 'getButtonState': if type(streaming_data) == 'tuple': buttons = streaming_data[idx] idx = idx + 1 else: # imu is only streaming button state, result is not a tuple buttons = streaming_data # publish streamed data pub[name].publish(imuMsg) pub[name + '_buttons'].publish(buttons) else: for name in imu_manager.imus: ## one message per imu imuMsg = Imu() imuMsg.header.stamp = timestamp imuMsg.header.frame_id = frame_id buttons = Int8() streaming_data = imu_manager.devices[ name].getStreamingBatch() idx = 0 imuMsg.orientation.x = streaming_data[idx] imuMsg.orientation.y = streaming_data[idx + 1] imuMsg.orientation.z = streaming_data[idx + 2] imuMsg.orientation.w = streaming_data[idx + 3] idx = idx + 4 imuMsg.angular_velocity.x = streaming_data[idx] imuMsg.angular_velocity.y = streaming_data[idx + 1] imuMsg.angular_velocity.z = streaming_data[idx + 2] idx = idx + 3 imuMsg.linear_acceleration.x = -streaming_data[idx] imuMsg.linear_acceleration.y = -streaming_data[idx + 1] imuMsg.linear_acceleration.z = -streaming_data[idx + 2] idx = idx + 3 buttons = streaming_data[idx] # publish streamed data pub[name].publish(imuMsg) pub[name + '_buttons'].publish(buttons) except TypeError: print 'TypeError occured!' # sleep until it's time to work again rate.sleep() # cleanup imu_manager.shutdown()
#!/usr/bin/env python import numpy as np import rospy from sensor_msgs.msg import Imu from tf.transformations import quaternion_from_euler from dynamic_reconfigure.server import Server from razor_imu_9dof.cfg import imuConfig from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue pub = rospy.Publisher('imu_transformed', Imu, queue_size=1) imu_topic = "imu_topic" imu_new = Imu() rospy.init_node('imu_tf') def scale_linear_accel(x, y, z): norm = np.sqrt(x**2+y**2+z**2) scale = 9.818/norm x_sc = scale*x y_sc = scale*y z_sc = scale*z return x_sc, y_sc, z_sc def callback_imu(imu): global imu_new imu_new = imu imu_new.header.stamp = rospy.get_rostime() imu_new.header.frame_id = "base_link" imu_new.orientation.x = imu.orientation.y
#! /usr/bin/env python import time import rospy import numpy as np from sensor_msgs.msg import Imu from tf.transformations import euler_from_quaternion, quaternion_from_euler from nav_msgs.msg import Odometry from geometry_msgs.msg import Vector3Stamped imu_node = Imu() class ubication(object): def __init__(self): self.pub = rospy.Publisher('/yaw', Imu, queue_size=10) #self.cuenta = 0 self.x = 0 self.y = 0 self.z = 0 self.yaw = 0 self.yaw_1 = 0 self.count = 0 #VARIABLE RORATION MATRIX self.acc = np.zeros([2, 1]) self.w = 0.0 self.w_1 = 0.0
import rospy import threading from std_msgs.msg import String from sensor_msgs.msg import Imu imu = Imu() lock = threading.Lock() def talker(): pub = rospy.Publisher('/camera/imu/data_raw', Imu, queue_size=10) rate = rospy.Rate(63) while not rospy.is_shutdown(): pub.publish(imu) rate.sleep() def callback_accel(data): with lock: imu.header.frame_id = data.header.frame_id imu.header.stamp = data.header.stamp imu.linear_acceleration.x = data.linear_acceleration.x imu.linear_acceleration.y = data.linear_acceleration.y imu.linear_acceleration.z = data.linear_acceleration.z def callback_gyro(data): with lock: imu.header.frame_id = data.header.frame_id
def __init__(self): #======== Get params =========# self.param = {} # create a dictionary #self.debug_mode = bool(rospy.get_param('~debug_mode', False)) # true for detail info self.param["vel_cmd"] = rospy.get_param("~cmd_vel", 'cmd_vel') self.param["odom_topic"] = rospy.get_param("~odom_topic", 'odom') self.param["imu_topic"] = rospy.get_param("~imu_topic", 'imu') self.param["baseId"] = rospy.get_param('~base_id', 'base_link') # base frame id self.param["odomId"] = rospy.get_param('~odom_id', 'odom') # odom frame id self.param["imuId"] = rospy.get_param('~imu_id', 'imu') # odom frame id self.param["enable_tf"] = rospy.get_param("~enable_tf", True) self.param["device_port"] = rospy.get_param('~port', '/dev/ttyS2') # port self.param["baudrate"] = int(rospy.get_param('~baudrate', '115200')) # baudrate self.param["timeout"] = float(rospy.get_param('~serial_timeout', '10')) # self.param["odom_freq"] = float(rospy.get_param( '~odom_freq', '30')) # hz of communication self.param["imu_freq"] = float(rospy.get_param( '~imu_freq', '120')) # hz of communication self.param["tx_freq"] = float(rospy.get_param( '~tx_freq', '5')) # hz of communication self.param["cmd_timeout"] = float( rospy.get_param('~cmd_vel_timeout', '3')) # self.param["vel_gain"] = float(rospy.get_param( '~vel_gain', '70')) # hz of communication self.param["omg_gain"] = float(rospy.get_param( '~omg_gain', '500')) # hz of communication rospy.set_param("omni_base_driver", self.param) #========== ROS message ==========# rospy.loginfo("Initiating Node") rospy.loginfo("Publishing odometry at: \"" + self.param["odom_topic"] + "\"") rospy.loginfo("Publishing imu at: \"" + self.param["imu_topic"] + "\"") rospy.loginfo("Subscribing to \"" + self.param["vel_cmd"] + "\"") self.data_handle_ok = False # prevent node from starting pre maturely #========== ROS handler ==========# self.enc_sub = rospy.Subscriber(self.param["vel_cmd"], Twist, self.velcmd_cb, queue_size=10) self.odom_pub = rospy.Publisher(self.param["odom_topic"], Odometry, queue_size=10) # pubisher self.imu_pub = rospy.Publisher(self.param["imu_topic"], Imu, queue_size=10) self.timer_odom = rospy.Timer( rospy.Duration(1.0 / self.param["odom_freq"]), self.odomPub) # timer self.timer_imu = rospy.Timer( rospy.Duration(1.0 / self.param["imu_freq"]), self.imuPub) self.timer_txcmd = rospy.Timer( rospy.Duration(1.0 / self.param["tx_freq"]), self.tx_vel_cmd) if self.param["enable_tf"]: rospy.loginfo("Publishing transform from %s to %s", self.param["odomId"], self.param["baseId"]) self.tf_br = tf.TransformBroadcaster() #========== variable for ROS publisher ==========# self.odom = Odometry() self.odom.header.seq = 0 self.odom.header.frame_id = self.param["odomId"] self.odom.child_frame_id = self.param["baseId"] for i in range(36): self.odom.pose.covariance[i] = 0 self.odom.twist.covariance[i] = 0 self.odom.twist.covariance[0] = 1.0 self.odom.twist.covariance[35] = 1.0 self.imu = Imu() self.imu.header.seq = 0 self.imu.header.frame_id = self.param["imuId"] self.imu.orientation.x = 0 self.imu.orientation.y = 0 self.imu.orientation.z = 0 self.imu.orientation.w = 0 for i in range(9): self.imu.orientation_covariance[i] = 0 self.imu.angular_velocity_covariance[i] = 0 self.imu.linear_acceleration_covariance[i] = 0 self.imu.orientation_covariance[0] = -1 self.imu.angular_velocity_covariance[0] = math.radians( 0.05) # P.12 of the MPU6050 datasheet self.imu.angular_velocity_covariance[4] = math.radians(0.05) self.imu.angular_velocity_covariance[8] = math.radians(0.05) self.imu.linear_acceleration_covariance[ 0] = 4000 * 10**-6 * 9.81 # in m/s**2 self.imu.linear_acceleration_covariance[4] = 4000 * 10**-6 * 9.81 self.imu.linear_acceleration_covariance[8] = 4000 * 10**-6 * 9.81 self.x_e = float(0) self.y_e = float(0) self.th = float(0) self.odom_last_seq = 0 self.enc_count_per_revo = (390 * 4) self.wheel_radius = 0.029 self.base_radius = 0.140 # 14.3 cm radius self.accel_sensitivity = 1.8 * 9.81 # 2g self.gyro_sensitivity = math.radians(250) # 250deg/sec self.last_odom_time = time.time() self.last_cmd_vel_time = self.last_odom_time self.no_cmd_received = True self.first_odom = True self.veh_cmd = {"Vx": 0, "Vy": 0, "Omega": 0}
def run(self): """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps. The IMU message, if fully filled in, contains information on orientation, acceleration (in m/s^2), and angular rate (in radians/sec). For each of these quantities, the IMU message format also wants the corresponding covariance matrix. Wiimote only gives us acceleration and angular rate. So we ensure that the orientation data entry is marked invalid. We do this by setting the first entry of its associated covariance matrix to -1. The covariance matrices are the 3x3 matrix with the axes' variance in the diagonal. We obtain the variance from the Wiimote instance. """ rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).") self.threadName = "IMU topic Publisher" try: while not rospy.is_shutdown(): (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData() msg = Imu( header=None, orientation=None, # will default to [0.,0.,0.,0], orientation_covariance=[ -1., 0., 0., 0., 0., 0., 0., 0., 0. ], # -1 indicates that orientation is unknown angular_velocity=None, angular_velocity_covariance=self. angular_velocity_covariance, linear_acceleration=None, linear_acceleration_covariance=self. linear_acceleration_covariance) # If a gyro is plugged into the Wiimote, then note the # angular velocity in the message, else indicate with # the special gyroAbsence_covariance matrix that angular # velocity is unavailable: if self.wiistate.motionPlusPresent: msg.angular_velocity.x = canonicalAngleRate[PHI] msg.angular_velocity.y = canonicalAngleRate[THETA] msg.angular_velocity.z = canonicalAngleRate[PSI] else: msg.angular_velocity_covariance = self.gyroAbsence_covariance msg.linear_acceleration.x = canonicalAccel[X] msg.linear_acceleration.y = canonicalAccel[Y] msg.linear_acceleration.z = canonicalAccel[Z] measureTime = self.wiistate.time timeSecs = int(measureTime) timeNSecs = int(abs(timeSecs - measureTime) * 10**9) msg.header.stamp.secs = timeSecs msg.header.stamp.nsecs = timeNSecs try: self.pub.publish(msg) except rospy.ROSException: rospy.loginfo( "Topic imu/data closed. Shutting down Imu sender.") exit(0) #rospy.logdebug("IMU state:") #rospy.logdebug(" IMU accel: " + str(canonicalAccel) + "\n IMU angular rate: " + str(canonicalAngleRate)) rospy.sleep(self.sleepDuration) except rospy.ROSInterruptException: rospy.loginfo("Shutdown request. Shutting down Imu sender.") exit(0)
def __init__(self, topic_name='/sphero/imu/data3'): self._topic_name = topic_name self._sub = rospy.Subscriber(self._topic_name, Imu, self.topic_callback) self._imu_data = Imu()
def __init__(self): rospy.init_node('interface', anonymous=False) rospy.loginfo("To stop TurtleBot CTRL + C") rospy.on_shutdown(self.shutdown) self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) rate = rospy.Rate(10) # Initialize tf listener, and give some time to fill its buffer self.tf_listener = tf.TransformListener() rospy.sleep(2) self.odom_frame = '/odom' self.base_frame = '/base_footprint' self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) self.scan_data = LaserScan() self.imu_data = Imu() rospy.Subscriber('scan', LaserScan, self.scan_callback) rospy.Subscriber('mobile_base/sensors/imu_data', Imu, self.imu_callback) MAPH = 600 MAPW = 600 PLENGTH = 0.05 # length per pixel SECURE_DIS = 0.5 # secure distance MAP_INFO = 5 # speed of map creating, namely how much information you want a laser point to change the map self.gmap = ones((MAPH, MAPW), dtype=uint8) * 128 yaw = 0 last_yaw = 0 yaw_flag = False while not rospy.is_shutdown(): try: (trans, rot) = self.tf_listener.lookupTransform( self.odom_frame, self.base_frame, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF Exception") # input: trans is position(x,y,z) # input: rot is angle(x,y,z,w) # input: self.scan_data is the laser data, include: # float32 angle_min # start angle of the scan [rad] # float32 angle_max # end angle of the scan [rad] # float32 angle_increment # angular distance between measurements [rad] # float32 time_increment # time between measurements [seconds] # float32 scan_time # time between scans [seconds] # float32 range_min # minimum range value [m] # float32 range_max # maximum range value [m] # float32[] ranges # range data [m] # float32[] intensities # intensity data [device-specific units] # input: self.imu_data is the imu data, include: # Quaternion orientation # Vector3 angular_velocity # Vector3 linear_acceleration #____________________________input end_________________________________ px = trans[0] py = trans[1] # global position mx = 0 my = 0 x = rot[0] y = rot[1] z = rot[2] w = rot[3] yaw = arctan((2 * (w * z + x * y)) / (1 - 2 * (y * y + z * z))) if (last_yaw * yaw < -1): yaw_flag = not yaw_flag last_yaw = yaw if (yaw_flag): yaw += pi # self.gmap = self.gmap*1 # when we get next scan frame, the map generated by last frame will be weaken scan_angle = self.scan_data.angle_min for r in self.scan_data.ranges: if (isnan(r)): scan_angle += self.scan_data.angle_increment continue angle = scan_angle + yaw gx = px + r * cos(angle) gy = py + r * sin(angle) # global position of laser point mpx = int(px / PLENGTH + MAPW / 2) mpy = int(py / PLENGTH + MAPH / 2) #if(mpx>=0 and mpx<MAPW and mpy>=0 and mpy<MAPH): # self.gmap[mpy,mpx]=128 # where is the robot now mx = int(gx / PLENGTH + MAPW / 2) my = int(gy / PLENGTH + MAPH / 2) if (mx < 0 or mx >= MAPW or my < 0 or my >= MAPH): scan_angle += self.scan_data.angle_increment continue # occupancy grid mapping delta_x = mx - mpx # calculate the grids where the laser line go through delta_y = my - mpy if (abs(delta_x) > abs(delta_y)): k = float(delta_y) / delta_x if (delta_x > 0): for x in range(mpx + 1, mx): ty = int(mpy + k * (x - mpx)) self.gmap[ty, x] = max(self.gmap[ty, x] - MAP_INFO, 0) else: for x in range(mx + 1, mpx): ty = int(my + k * (x - mx)) self.gmap[ty, x] = max(self.gmap[ty, x] - MAP_INFO, 0) else: k = float(delta_x) / delta_y if (delta_y > 0): for y in range(mpy + 1, my): tx = int(mpx + k * (y - mpy)) self.gmap[y, tx] = max(self.gmap[y, tx] - MAP_INFO, 0) else: for y in range(my + 1, mpy): tx = int(mx + k * (y - my)) self.gmap[y, tx] = max(self.gmap[y, tx] - MAP_INFO, 0) self.gmap[my, mx] = min(self.gmap[my, mx] + MAP_INFO, 255) scan_angle += self.scan_data.angle_increment cv2.imwrite("map.png", cv2.flip(self.gmap, 0)) #___________________________mapping end________________________________ speed = 0.2 steer = 0 # speed and steer is the variable to output MAX_D = self.gmap.shape[0] + self.gmap.shape[1] obs = argwhere(self.gmap > 200) # where are obstacles min_d = MAX_D # find which obstacle point is the nearest mx = int(px / PLENGTH + MAPW / 2) my = int(py / PLENGTH + MAPH / 2) for p in obs: if ((p[1] - mx) * cos(yaw) + (p[0] - my) * sin(yaw) < 0): continue # the inner product of (p[1]-mx,p[0]-my) and yaw should be positve dis = sqrt((my - p[0])**2 + (mx - p[1])**2) if (min_d > dis): min_d = dis min_d = min_d * PLENGTH if (len(obs)): speed = ( min_d - SECURE_DIS ) * 0.5 # speed will slow down when robot near obstacles else: speed = 0 # speed will be zero when we haven't got the laser data if (speed > 0.5): speed = 0.5 if (speed < 0.01): speed = 0 alter_steer = [0, 0.2, -0.2, 0.4, -0.4, 0.6, -0.6] DT = 0.5 # prediction precision max_eva = 0 for s in alter_steer: # choose the best steer in alter set future_x = px future_y = py future_yaw = yaw eva = 0 for t in range( 6 ): # predict the sport of robot in next 3 seconds by evaluate function dis = speed * DT future_x = future_x + dis * cos(future_yaw) future_y = future_y + dis * sin(future_yaw) future_yaw = future_yaw + s * DT mx = int(future_x / PLENGTH + MAPW / 2) my = int(future_y / PLENGTH + MAPH / 2) eva += self.evaluate(my, mx, future_yaw) if (eva > max_eva): max_eva = eva steer = s if (speed == 0): steer = 0.5 # if robot is not move ,rotate it to find a way #____________________________output begin_________________________________ move_cmd = Twist() print((speed, steer)) move_cmd.linear.x = speed move_cmd.angular.z = steer self.cmd_vel.publish(move_cmd) rate.sleep()
from mavros_msgs.srv import SetMode from mavros_msgs.srv import SetModeRequest from mavros_msgs.srv import SetModeResponse from mavros_msgs.srv import CommandBool from mavros_msgs.srv import CommandBoolRequest from mavros_msgs.srv import CommandBoolResponse from sensor_msgs.msg import Imu import argparse parser = argparse.ArgumentParser() parser.add_argument("data_points", help="data_points", type=int) args = parser.parse_args() data_points = args.data_points current_state = State() imu_data = Imu() act_controls = ActuatorControl() PI = 3.14 k_p_yaw = 0.05 k_d_yaw = 0 k_p_pitch = 0.18 k_d_pitch = 0.045 k_p_roll = 0.18 k_d_roll = 0.045 x_f = [] y_f = []
#! /usr/bin/env python3 import airsim import rospy import numpy as np from sensor_msgs.msg import Imu client = airsim.MultirotorClient() client.confirmConnection() #Initializing ros node and publisher rospy.init_node('airsim_imu_node') imu_pub = rospy.Publisher('airsim/Imu', Imu, queue_size=1) #Initializing IMU message ros_imu = Imu() r = rospy.Rate(200.0) try: while not rospy.is_shutdown(): airsim_imu = client.getImuData(imu_name="Imu", vehicle_name="") #Preparing IMU message ros_imu.angular_velocity.x = airsim_imu.angular_velocity.x_val ros_imu.angular_velocity.y = airsim_imu.angular_velocity.y_val ros_imu.angular_velocity.z = -1 * airsim_imu.angular_velocity.z_val ros_imu.linear_acceleration.x = airsim_imu.linear_acceleration.x_val ros_imu.linear_acceleration.y = -1 * airsim_imu.linear_acceleration.y_val ros_imu.linear_acceleration.z = -1 * airsim_imu.linear_acceleration.z_val ros_imu.orientation.w = airsim_imu.orientation.w_val
(config['yaw_calibration'])) #if imu_yaw_calibration != config('yaw_calibration'): imu_yaw_calibration = config['yaw_calibration'] rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration)) return config rospy.init_node("razor_node") #We only care about the most recent measurement, i.e. queue_size=1 pub = rospy.Publisher('imu/data', Imu, queue_size=1) srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1) diag_pub_time = rospy.get_time() imuMsg = Imu() # Orientation covariance estimation: # Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z # Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss # Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could # cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians # i.e. variance in yaw: 0.0025 # Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause # static roll/pitch error of 0.8%, owing to gravity orientation sensing # error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025 # so set all covariances the same. imuMsg.orientation_covariance = [0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025] # Angular velocity covariance estimation: # Observed gyro noise: 4 counts => 0.28 degrees/sec
def ser_rw(self): # initializations pub1 = rospy.Publisher('imu/data_raw1', Imu, queue_size=100) pub2 = rospy.Publisher('imu/data_raw2', Imu, queue_size=100) pub3 = rospy.Publisher('imu/data_raw3', Imu, queue_size=100) rospy.Subscriber("ser_wrt", String, self.wrt_cb) rt = rospy.Rate(self.ser_rate) # imu message imu_msg = Imu() # Port setup try: ser = serial.Serial(self.port_name, self.baud, timeout=0.01) #ser.close() #ser.open() self.serflag = True rospy.loginfo('Serial port access successful') time.sleep(3) except rospy.ROSInterruptException: rospy.loginfo('Error in opening serial port') rospy.signal_shutdown('Shutting down') #index variables beg = 0 en = 0 rospy.loginfo(self.serflag) if self.serflag: # Flush the serial port once in the beginning ser.read(ser.inWaiting()) rospy.loginfo('Serial port flushed') rospy.loginfo(ser.write('IMU3')) # Loop begin while not rospy.is_shutdown(): #Check if there is data to be written and write data if self.wrtflag: ser.write(self.wrt_buf) self.wrtflag = False #Check if there is data to be read, if so write if ser.inWaiting(): self.read_buf = self.read_buf + ser.read(ser.inWaiting()) #print('read') if len(self.read_buf) >= self.word_len: beg = self.read_buf.find('I') #print(len(self.read_buf)) self.read_buf = self.read_buf[beg:] #print(len(self.read_buf)) #print('word_len received') #print(self.read_buf) ti = rospy.get_time() if (len(self.read_buf) >= self.word_len and self.read_buf[self.word_len - 1] == 'U'): imu_msg.angular_velocity.x = int( self.read_buf[20:26]) * self.avel_scl imu_msg.angular_velocity.y = int( self.read_buf[26:32]) * self.avel_scl imu_msg.angular_velocity.z = int( self.read_buf[32:38]) * self.avel_scl imu_msg.linear_acceleration.x = int( self.read_buf[2:8]) * self.acc_scl imu_msg.linear_acceleration.y = int( self.read_buf[8:14]) * self.acc_scl imu_msg.linear_acceleration.z = int( self.read_buf[14:20]) * self.acc_scl #print('pub') if (self.read_buf[1] == '0'): pub1.publish(imu_msg) elif (self.read_buf[1] == '1'): pub2.publish(imu_msg) elif (self.read_buf[1] == '2'): pub3.publish(imu_msg) self.read_buf = self.read_buf[self.word_len:] #print(rospy.get_time()-ti) #print(len(self.read_buf)) rt.sleep() ser.write('IMU0')