|
@@ -30,7 +30,7 @@ degrees2rad = math.pi/180.0
|
|
imu_yaw_calibration = 0.0
|
|
imu_yaw_calibration = 0.0
|
|
yaw = 0.0
|
|
yaw = 0.0
|
|
|
|
|
|
-# Server return Yaw data
|
|
|
|
|
|
+# ros server return Yaw data
|
|
def return_yaw_data(req):
|
|
def return_yaw_data(req):
|
|
return GetYawDataResponse(yaw)
|
|
return GetYawDataResponse(yaw)
|
|
|
|
|
|
@@ -109,9 +109,9 @@ while not rospy.is_shutdown():
|
|
# Publish imu message
|
|
# Publish imu message
|
|
#rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
|
|
#rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
|
|
# myIMU.raw_ay, myIMU.raw_az)
|
|
# myIMU.raw_ay, myIMU.raw_az)
|
|
- imuMsg.linear_acceleration.x = float(myIMU.raw_ax)*accel_factor
|
|
|
|
- imuMsg.linear_acceleration.y = float(myIMU.raw_ay)*accel_factor
|
|
|
|
- imuMsg.linear_acceleration.z = float(myIMU.raw_az)*accel_factor
|
|
|
|
|
|
+ imuMsg.linear_acceleration.x = -float(myIMU.raw_ax)*accel_factor
|
|
|
|
+ imuMsg.linear_acceleration.y = -float(myIMU.raw_ay)*accel_factor
|
|
|
|
+ imuMsg.linear_acceleration.z = -float(myIMU.raw_az)*accel_factor
|
|
|
|
|
|
imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
|
|
imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
|
|
imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
|
|
imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
|