# 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')
