From b7775e9610a626e2643f4d9b2db4f311c3960b51 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Thu, 17 Oct 2019 16:43:44 +0200 Subject: [PATCH 01/18] Update .gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 9a97992..359920d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ *~ config/my_razor.yaml +*.pyc +*.swp From be7e2d9abfd68c0bcc28e9fc3fd03b2cd1641291 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Thu, 17 Oct 2019 19:11:36 +0200 Subject: [PATCH 02/18] WIP refactoring IMU driver --- nodes/imu_node.py | 314 ++++++++++++++++++---------------- nodes/lib/__init__.py | 0 nodes/lib/serial_commands.py | 47 +++++ src/Razor_AHRS/Razor_AHRS.ino | 2 +- 4 files changed, 215 insertions(+), 148 deletions(-) create mode 100644 nodes/lib/__init__.py create mode 100644 nodes/lib/serial_commands.py diff --git a/nodes/imu_node.py b/nodes/imu_node.py index 27a4275..aa5b19d 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -32,6 +32,8 @@ import string import math import sys +from time import sleep +from lib.serial_commands import * #from time import time from sensor_msgs.msg import Imu @@ -52,12 +54,20 @@ def reconfig_callback(config, level): rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration)) return config +def send_command(serial_instance, command): + command = command + chr(13) + expected_len = len(command) + res = serial_instance.write(command) + if (res != expected_len): + rospy.logerr("Expected serial command len (%d) didn't match amount of bytes written (%d) for command %s", expected_len, res, command) + sleep(0.05) # Don't spam serial with too many commands at once + rospy.init_node("razor_node") #We only care about the most recent measurement, i.e. queue_size=1 pub = rospy.Publisher('imu', 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(); +diag_pub_time = rospy.get_time() imuMsg = Imu() @@ -101,32 +111,42 @@ def reconfig_callback(config, level): port = rospy.get_param('~port', default_port) #read calibration parameters -port = rospy.get_param('~port', default_port) + +calib_dict = {} #accelerometer -accel_x_min = rospy.get_param('~accel_x_min', -250.0) -accel_x_max = rospy.get_param('~accel_x_max', 250.0) -accel_y_min = rospy.get_param('~accel_y_min', -250.0) -accel_y_max = rospy.get_param('~accel_y_max', 250.0) -accel_z_min = rospy.get_param('~accel_z_min', -250.0) -accel_z_max = rospy.get_param('~accel_z_max', 250.0) +calib_dict["accel_x_min"] = rospy.get_param('~accel_x_min', -250.0) +calib_dict["accel_x_max"] = rospy.get_param('~accel_x_max', 250.0) +calib_dict["accel_y_min"] = rospy.get_param('~accel_y_min', -250.0) +calib_dict["accel_y_max"] = rospy.get_param('~accel_y_max', 250.0) +calib_dict["accel_z_min"] = rospy.get_param('~accel_z_min', -250.0) +calib_dict["accel_z_max"] = rospy.get_param('~accel_z_max', 250.0) # magnetometer -magn_x_min = rospy.get_param('~magn_x_min', -600.0) -magn_x_max = rospy.get_param('~magn_x_max', 600.0) -magn_y_min = rospy.get_param('~magn_y_min', -600.0) -magn_y_max = rospy.get_param('~magn_y_max', 600.0) -magn_z_min = rospy.get_param('~magn_z_min', -600.0) -magn_z_max = rospy.get_param('~magn_z_max', 600.0) -calibration_magn_use_extended = rospy.get_param('~calibration_magn_use_extended', False) -magn_ellipsoid_center = rospy.get_param('~magn_ellipsoid_center', [0, 0, 0]) -magn_ellipsoid_transform = rospy.get_param('~magn_ellipsoid_transform', [[0, 0, 0], [0, 0, 0], [0, 0, 0]]) -imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0) +calib_dict["magn_x_min"] = rospy.get_param('~magn_x_min', -600.0) +calib_dict["magn_x_max"] = rospy.get_param('~magn_x_max', 600.0) +calib_dict["magn_y_min"] = rospy.get_param('~magn_y_min', -600.0) +calib_dict["magn_y_max"] = rospy.get_param('~magn_y_max', 600.0) +calib_dict["magn_z_min"] = rospy.get_param('~magn_z_min', -600.0) +calib_dict["magn_z_max"] = rospy.get_param('~magn_z_max', 600.0) +calib_dict["magn_use_extended"] = rospy.get_param('~calibration_magn_use_extended', False) +calib_dict["magn_ellipsoid_center"] = rospy.get_param('~magn_ellipsoid_center', [0, 0, 0]) +calib_dict["magn_ellipsoid_transform"] = rospy.get_param('~magn_ellipsoid_transform', [[0, 0, 0], [0, 0, 0], [0, 0, 0]]) # gyroscope -gyro_average_offset_x = rospy.get_param('~gyro_average_offset_x', 0.0) -gyro_average_offset_y = rospy.get_param('~gyro_average_offset_y', 0.0) -gyro_average_offset_z = rospy.get_param('~gyro_average_offset_z', 0.0) +calib_dict["gyro_average_offset_x"] = rospy.get_param('~gyro_average_offset_x', 0.0) +calib_dict["gyro_average_offset_y"] = rospy.get_param('~gyro_average_offset_y', 0.0) +calib_dict["gyro_average_offset_z"] = rospy.get_param('~gyro_average_offset_z', 0.0) + +imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0) + +# variables for calibration verification +verify_calibration = rospy.get_param('~verify_calibration', True) +calibration_file = rospy.get_param('~calibration_path', None) #set this in the launch file + +if verify_calibration and calibration_file is None: + rospy.logerr("Verify calibration has been enabled but no calibration path was provided") + exit(-1) #rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max) #rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max) @@ -147,137 +167,137 @@ def reconfig_callback(config, level): yaw=0 seq=0 accel_factor = 9.806 / 256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2. -rospy.loginfo("Giving the razor IMU board 5 seconds to boot...") -rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot +# rospy.loginfo("Giving the razor IMU board 5 seconds to boot...") +# rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot ### configure board ### #stop datastream -ser.write('#o0' + chr(13)) +send_command(ser, STOP_DATASTREAM) #discard old input #automatic flush - NOT WORKING #ser.flushInput() #discard old input, still in invalid format -#flush manually, as above command is not working -discard = ser.readlines() - -#set output mode -ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text - -rospy.loginfo("Writing calibration values to razor IMU board...") -#set calibration values -ser.write('#caxm' + str(accel_x_min) + chr(13)) -ser.write('#caxM' + str(accel_x_max) + chr(13)) -ser.write('#caym' + str(accel_y_min) + chr(13)) -ser.write('#cayM' + str(accel_y_max) + chr(13)) -ser.write('#cazm' + str(accel_z_min) + chr(13)) -ser.write('#cazM' + str(accel_z_max) + chr(13)) - -if (not calibration_magn_use_extended): - ser.write('#cmxm' + str(magn_x_min) + chr(13)) - ser.write('#cmxM' + str(magn_x_max) + chr(13)) - ser.write('#cmym' + str(magn_y_min) + chr(13)) - ser.write('#cmyM' + str(magn_y_max) + chr(13)) - ser.write('#cmzm' + str(magn_z_min) + chr(13)) - ser.write('#cmzM' + str(magn_z_max) + chr(13)) -else: - ser.write('#ccx' + str(magn_ellipsoid_center[0]) + chr(13)) - ser.write('#ccy' + str(magn_ellipsoid_center[1]) + chr(13)) - ser.write('#ccz' + str(magn_ellipsoid_center[2]) + chr(13)) - ser.write('#ctxX' + str(magn_ellipsoid_transform[0][0]) + chr(13)) - ser.write('#ctxY' + str(magn_ellipsoid_transform[0][1]) + chr(13)) - ser.write('#ctxZ' + str(magn_ellipsoid_transform[0][2]) + chr(13)) - ser.write('#ctyX' + str(magn_ellipsoid_transform[1][0]) + chr(13)) - ser.write('#ctyY' + str(magn_ellipsoid_transform[1][1]) + chr(13)) - ser.write('#ctyZ' + str(magn_ellipsoid_transform[1][2]) + chr(13)) - ser.write('#ctzX' + str(magn_ellipsoid_transform[2][0]) + chr(13)) - ser.write('#ctzY' + str(magn_ellipsoid_transform[2][1]) + chr(13)) - ser.write('#ctzZ' + str(magn_ellipsoid_transform[2][2]) + chr(13)) - -ser.write('#cgx' + str(gyro_average_offset_x) + chr(13)) -ser.write('#cgy' + str(gyro_average_offset_y) + chr(13)) -ser.write('#cgz' + str(gyro_average_offset_z) + chr(13)) - -#print calibration values for verification by user -ser.flushInput() -ser.write('#p' + chr(13)) -calib_data = ser.readlines() -calib_data_print = "Printing set calibration values:\r\n" -for line in calib_data: - calib_data_print += line -rospy.loginfo(calib_data_print) - -#start datastream -ser.write('#o1' + chr(13)) - -#automatic flush - NOT WORKING -#ser.flushInput() #discard old input, still in invalid format -#flush manually, as above command is not working - it breaks the serial connection -rospy.loginfo("Flushing first 200 IMU entries...") -for x in range(0, 200): - line = ser.readline() -rospy.loginfo("Publishing IMU data...") -#f = open("raw_imu_data.log", 'w') - -while not rospy.is_shutdown(): - line = ser.readline() - line = line.replace("#YPRAG=","") # Delete "#YPRAG=" - #f.write(line) # Write to the output log file - words = string.split(line,",") # Fields split - if len(words) > 2: - #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) - yaw_deg = -float(words[0]) - yaw_deg = yaw_deg + imu_yaw_calibration - if yaw_deg > 180.0: - yaw_deg = yaw_deg - 360.0 - if yaw_deg < -180.0: - yaw_deg = yaw_deg + 360.0 - yaw = yaw_deg*degrees2rad - #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) - pitch = -float(words[1])*degrees2rad - roll = float(words[2])*degrees2rad - - # Publish message - # AHRS firmware accelerations are negated - # This means y and z are correct for ROS, but x needs reversing - imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor - imuMsg.linear_acceleration.y = float(words[4]) * accel_factor - imuMsg.linear_acceleration.z = float(words[5]) * accel_factor - - imuMsg.angular_velocity.x = float(words[6]) - #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) - imuMsg.angular_velocity.y = -float(words[7]) - #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) - imuMsg.angular_velocity.z = -float(words[8]) - - q = quaternion_from_euler(roll,pitch,yaw) - imuMsg.orientation.x = q[0] - imuMsg.orientation.y = q[1] - imuMsg.orientation.z = q[2] - imuMsg.orientation.w = q[3] - imuMsg.header.stamp= rospy.Time.now() - imuMsg.header.frame_id = 'base_imu_link' - imuMsg.header.seq = seq - seq = seq + 1 - pub.publish(imuMsg) - - if (diag_pub_time < rospy.get_time()) : - diag_pub_time += 1 - diag_arr = DiagnosticArray() - diag_arr.header.stamp = rospy.get_rostime() - diag_arr.header.frame_id = '1' - diag_msg = DiagnosticStatus() - diag_msg.name = 'Razor_Imu' - diag_msg.level = DiagnosticStatus.OK - diag_msg.message = 'Received AHRS measurement' - diag_msg.values.append(KeyValue('roll (deg)', - str(roll*(180.0/math.pi)))) - diag_msg.values.append(KeyValue('pitch (deg)', - str(pitch*(180.0/math.pi)))) - diag_msg.values.append(KeyValue('yaw (deg)', - str(yaw*(180.0/math.pi)))) - diag_msg.values.append(KeyValue('sequence number', str(seq))) - diag_arr.status.append(diag_msg) - diag_pub.publish(diag_arr) +# #flush manually, as above command is not working +# discard = ser.readlines() + +# #set output mode +# ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text + +# rospy.loginfo("Writing calibration values to razor IMU board...") +# #set calibration values +# ser.write('#caxm' + str(accel_x_min) + chr(13)) +# ser.write('#caxM' + str(accel_x_max) + chr(13)) +# ser.write('#caym' + str(accel_y_min) + chr(13)) +# ser.write('#cayM' + str(accel_y_max) + chr(13)) +# ser.write('#cazm' + str(accel_z_min) + chr(13)) +# ser.write('#cazM' + str(accel_z_max) + chr(13)) + +# if (not calibration_magn_use_extended): +# ser.write('#cmxm' + str(magn_x_min) + chr(13)) +# ser.write('#cmxM' + str(magn_x_max) + chr(13)) +# ser.write('#cmym' + str(magn_y_min) + chr(13)) +# ser.write('#cmyM' + str(magn_y_max) + chr(13)) +# ser.write('#cmzm' + str(magn_z_min) + chr(13)) +# ser.write('#cmzM' + str(magn_z_max) + chr(13)) +# else: +# ser.write('#ccx' + str(magn_ellipsoid_center[0]) + chr(13)) +# ser.write('#ccy' + str(magn_ellipsoid_center[1]) + chr(13)) +# ser.write('#ccz' + str(magn_ellipsoid_center[2]) + chr(13)) +# ser.write('#ctxX' + str(magn_ellipsoid_transform[0][0]) + chr(13)) +# ser.write('#ctxY' + str(magn_ellipsoid_transform[0][1]) + chr(13)) +# ser.write('#ctxZ' + str(magn_ellipsoid_transform[0][2]) + chr(13)) +# ser.write('#ctyX' + str(magn_ellipsoid_transform[1][0]) + chr(13)) +# ser.write('#ctyY' + str(magn_ellipsoid_transform[1][1]) + chr(13)) +# ser.write('#ctyZ' + str(magn_ellipsoid_transform[1][2]) + chr(13)) +# ser.write('#ctzX' + str(magn_ellipsoid_transform[2][0]) + chr(13)) +# ser.write('#ctzY' + str(magn_ellipsoid_transform[2][1]) + chr(13)) +# ser.write('#ctzZ' + str(magn_ellipsoid_transform[2][2]) + chr(13)) + +# ser.write('#cgx' + str(gyro_average_offset_x) + chr(13)) +# ser.write('#cgy' + str(gyro_average_offset_y) + chr(13)) +# ser.write('#cgz' + str(gyro_average_offset_z) + chr(13)) + +# #print calibration values for verification by user +# ser.flushInput() +# ser.write('#p' + chr(13)) +# calib_data = ser.readlines() +# calib_data_print = "Printing set calibration values:\r\n" +# for line in calib_data: +# calib_data_print += line +# rospy.loginfo(calib_data_print) + +# #start datastream +# ser.write('#o1' + chr(13)) + +# #automatic flush - NOT WORKING +# #ser.flushInput() #discard old input, still in invalid format +# #flush manually, as above command is not working - it breaks the serial connection +# rospy.loginfo("Flushing first 200 IMU entries...") +# for x in range(0, 200): +# line = ser.readline() +# rospy.loginfo("Publishing IMU data...") +# #f = open("raw_imu_data.log", 'w') + +# while not rospy.is_shutdown(): +# line = ser.readline() +# line = line.replace("#YPRAG=","") # Delete "#YPRAG=" +# #f.write(line) # Write to the output log file +# words = string.split(line,",") # Fields split +# if len(words) > 2: +# #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) +# yaw_deg = -float(words[0]) +# yaw_deg = yaw_deg + imu_yaw_calibration +# if yaw_deg > 180.0: +# yaw_deg = yaw_deg - 360.0 +# if yaw_deg < -180.0: +# yaw_deg = yaw_deg + 360.0 +# yaw = yaw_deg*degrees2rad +# #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) +# pitch = -float(words[1])*degrees2rad +# roll = float(words[2])*degrees2rad + +# # Publish message +# # AHRS firmware accelerations are negated +# # This means y and z are correct for ROS, but x needs reversing +# imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor +# imuMsg.linear_acceleration.y = float(words[4]) * accel_factor +# imuMsg.linear_acceleration.z = float(words[5]) * accel_factor + +# imuMsg.angular_velocity.x = float(words[6]) +# #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) +# imuMsg.angular_velocity.y = -float(words[7]) +# #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) +# imuMsg.angular_velocity.z = -float(words[8]) + +# q = quaternion_from_euler(roll,pitch,yaw) +# imuMsg.orientation.x = q[0] +# imuMsg.orientation.y = q[1] +# imuMsg.orientation.z = q[2] +# imuMsg.orientation.w = q[3] +# imuMsg.header.stamp= rospy.Time.now() +# imuMsg.header.frame_id = 'base_imu_link' +# imuMsg.header.seq = seq +# seq = seq + 1 +# pub.publish(imuMsg) + +# if (diag_pub_time < rospy.get_time()) : +# diag_pub_time += 1 +# diag_arr = DiagnosticArray() +# diag_arr.header.stamp = rospy.get_rostime() +# diag_arr.header.frame_id = '1' +# diag_msg = DiagnosticStatus() +# diag_msg.name = 'Razor_Imu' +# diag_msg.level = DiagnosticStatus.OK +# diag_msg.message = 'Received AHRS measurement' +# diag_msg.values.append(KeyValue('roll (deg)', +# str(roll*(180.0/math.pi)))) +# diag_msg.values.append(KeyValue('pitch (deg)', +# str(pitch*(180.0/math.pi)))) +# diag_msg.values.append(KeyValue('yaw (deg)', +# str(yaw*(180.0/math.pi)))) +# diag_msg.values.append(KeyValue('sequence number', str(seq))) +# diag_arr.status.append(diag_msg) +# diag_pub.publish(diag_arr) -ser.close -#f.close +# ser.close +# #f.close diff --git a/nodes/lib/__init__.py b/nodes/lib/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/nodes/lib/serial_commands.py b/nodes/lib/serial_commands.py new file mode 100644 index 0000000..c20bac5 --- /dev/null +++ b/nodes/lib/serial_commands.py @@ -0,0 +1,47 @@ +STOP_DATASTREAM = '#o0' +START_DATASTREAM = '#o1' + +GET_CALIBRATION_VALUES ='#p' + +SET_BINARY_FORMAT = '#ob' +SET_TEXT_FORMAT = '#ot' +SET_TEXT_EXTENDED_FORMAT = '#ox' # Use this one in the ROS driver + +SET_CALIBRATION_OUTPUT_MODE = '#oc' +NEXT_SENSOR_CALIBRATION = '#on' + +ENABLE_ERROR_MESSAGE = '#oe1' +DISABLE_ERROR_MESSAGE = '#oe0' +OUTPUT_ERROR_COUNT = '#oec' +OUTPUT_MATH_ERROR_COUNT = '#oem' + +SET_CALIB_ACC_X_MIN = '#caxm' +SET_CALIB_ACC_X_MAX = '#caxM' +SET_CALIB_ACC_Y_MIN = '#caym' +SET_CALIB_ACC_Y_MAX = '#cayM' +SET_CALIB_ACC_Z_MIN = '#cazm' +SET_CALIB_ACC_Z_MAX = '#cazM' + +SET_MAG_X_MIN = '#cmxm' +SET_MAG_X_MAX = '#cmxM' +SET_MAG_Y_MIN = '#cmym' +SET_MAG_Y_MAX = '#cmyM' +SET_MAG_Z_MIN = '#cmzm' +SET_MAG_Z_MAX = '#cmzM' + +SET_MAG_ELLIPSOID_CENTER_0 = '#ccx' +SET_MAG_ELLIPSOID_CENTER_1 = '#ccy' +SET_MAG_ELLIPSOID_CENTER_2 = '#ccz' +SET_MAG_ELLIPSOID_TRANSFORM_0_0 = '#ctxX' +SET_MAG_ELLIPSOID_TRANSFORM_0_1 = '#ctxY' +SET_MAG_ELLIPSOID_TRANSFORM_0_2 = '#ctxZ' +SET_MAG_ELLIPSOID_TRANSFORM_1_0 = '#ctyX' +SET_MAG_ELLIPSOID_TRANSFORM_1_1 = '#ctyY' +SET_MAG_ELLIPSOID_TRANSFORM_1_2 = '#ctyZ' +SET_MAG_ELLIPSOID_TRANSFORM_2_0 = '#ctzX' +SET_MAG_ELLIPSOID_TRANSFORM_2_1 = '#ctzY' +SET_MAG_ELLIPSOID_TRANSFORM_2_2 = '#ctzZ' + +SET_GYRO_AVERAGE_OFFSET_X = '#cgx' +SET_GYRO_AVERAGE_OFFSET_Y = '#cgy' +SET_GYRO_AVERAGE_OFFSET_Z = '#cgz' \ No newline at end of file diff --git a/src/Razor_AHRS/Razor_AHRS.ino b/src/Razor_AHRS/Razor_AHRS.ino index 13b113c..f794576 100644 --- a/src/Razor_AHRS/Razor_AHRS.ino +++ b/src/Razor_AHRS/Razor_AHRS.ino @@ -208,7 +208,7 @@ // Select your hardware here by uncommenting one line! //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer) -//#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" +#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer) From 083820b51c4813e878ddf2aded03884baa4bf4eb Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Sat, 19 Oct 2019 13:45:59 +0200 Subject: [PATCH 03/18] Write and check the calibration written to the sensor --- nodes/imu_node.py | 92 +++++++++++++++++++++++++++++------------------ 1 file changed, 57 insertions(+), 35 deletions(-) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index aa5b19d..6cc5a9b 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -34,6 +34,7 @@ import sys from time import sleep from lib.serial_commands import * +import yaml #from time import time from sensor_msgs.msg import Imu @@ -54,14 +55,64 @@ def reconfig_callback(config, level): rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration)) return config -def send_command(serial_instance, command): - command = command + chr(13) - expected_len = len(command) - res = serial_instance.write(command) +def send_command(serial_instance, command, value=None): + if value is None: + cmd = command + chr(13) + else: + cmd = command + str(value) + chr(13) + rospy.loginfo("Sending: %s", cmd) + expected_len = len(cmd) + res = serial_instance.write(cmd) if (res != expected_len): rospy.logerr("Expected serial command len (%d) didn't match amount of bytes written (%d) for command %s", expected_len, res, command) sleep(0.05) # Don't spam serial with too many commands at once +def write_and_check_config(serial_instance, calib_dict): + send_command(serial_instance, SET_CALIB_ACC_X_MIN, calib_dict["accel_x_min"]) + send_command(serial_instance, SET_CALIB_ACC_X_MAX, calib_dict["accel_x_max"]) + send_command(serial_instance, SET_CALIB_ACC_Y_MIN, calib_dict["accel_y_min"]) + send_command(serial_instance, SET_CALIB_ACC_Y_MAX, calib_dict["accel_y_max"]) + send_command(serial_instance, SET_CALIB_ACC_Z_MIN, calib_dict["accel_z_min"]) + send_command(serial_instance, SET_CALIB_ACC_Z_MAX, calib_dict["accel_z_max"]) + + if not calib_dict["magn_use_extended"]: + send_command(serial_instance, SET_MAG_X_MIN, calib_dict["magn_x_min"]) + send_command(serial_instance, SET_MAG_X_MAX, calib_dict["magn_x_max"]) + send_command(serial_instance, SET_MAG_Y_MIN, calib_dict["magn_y_min"]) + send_command(serial_instance, SET_MAG_Y_MAX, calib_dict["magn_y_max"]) + send_command(serial_instance, SET_MAG_Z_MIN, calib_dict["magn_z_min"]) + send_command(serial_instance, SET_MAG_Z_MAX, calib_dict["magn_z_max"]) + else: + send_command(serial_instance, SET_MAG_ELLIPSOID_CENTER_0, calib_dict["magn_ellipsoid_center"][0]) + send_command(serial_instance, SET_MAG_ELLIPSOID_CENTER_1, calib_dict["magn_ellipsoid_center"][1]) + send_command(serial_instance, SET_MAG_ELLIPSOID_CENTER_2, calib_dict["magn_ellipsoid_center"][2]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_0_0, calib_dict["magn_ellipsoid_transform"][0][0]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_0_1, calib_dict["magn_ellipsoid_transform"][0][1]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_0_2, calib_dict["magn_ellipsoid_transform"][0][2]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_1_0, calib_dict["magn_ellipsoid_transform"][1][0]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_1_1, calib_dict["magn_ellipsoid_transform"][1][1]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_1_2, calib_dict["magn_ellipsoid_transform"][1][2]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_2_0, calib_dict["magn_ellipsoid_transform"][2][0]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_2_1, calib_dict["magn_ellipsoid_transform"][2][1]) + send_command(serial_instance, SET_MAG_ELLIPSOID_TRANSFORM_2_2, calib_dict["magn_ellipsoid_transform"][2][2]) + + send_command(serial_instance, SET_GYRO_AVERAGE_OFFSET_X, calib_dict["gyro_average_offset_x"]) + send_command(serial_instance, SET_GYRO_AVERAGE_OFFSET_Y, calib_dict["gyro_average_offset_y"]) + send_command(serial_instance, SET_GYRO_AVERAGE_OFFSET_Z, calib_dict["gyro_average_offset_z"]) + + send_command(serial_instance, GET_CALIBRATION_VALUES) + config = "" + for _ in range(0, 21): + # Format each line received from serial into proper yaml with lowercase variable names + config += serial_instance.readline().lower().replace(":", ": ") + # TODO: round the numbers, otherwise we will get false negatives in the check phase + + config_parsed = yaml.load(config) + for key in calib_dict: + if config_parsed[key] != calib_dict[key]: + rospy.logwarn("The calibration value of [%s] did not match. Expected: %s, received: %s", + key, str(calib_dict[key]), str(config_parsed[key])) + rospy.init_node("razor_node") #We only care about the most recent measurement, i.e. queue_size=1 pub = rospy.Publisher('imu', Imu, queue_size=1) @@ -173,6 +224,7 @@ def send_command(serial_instance, command): ### configure board ### #stop datastream send_command(ser, STOP_DATASTREAM) +write_and_check_config(ser, calib_dict) #discard old input #automatic flush - NOT WORKING @@ -185,37 +237,7 @@ def send_command(serial_instance, command): # rospy.loginfo("Writing calibration values to razor IMU board...") # #set calibration values -# ser.write('#caxm' + str(accel_x_min) + chr(13)) -# ser.write('#caxM' + str(accel_x_max) + chr(13)) -# ser.write('#caym' + str(accel_y_min) + chr(13)) -# ser.write('#cayM' + str(accel_y_max) + chr(13)) -# ser.write('#cazm' + str(accel_z_min) + chr(13)) -# ser.write('#cazM' + str(accel_z_max) + chr(13)) - -# if (not calibration_magn_use_extended): -# ser.write('#cmxm' + str(magn_x_min) + chr(13)) -# ser.write('#cmxM' + str(magn_x_max) + chr(13)) -# ser.write('#cmym' + str(magn_y_min) + chr(13)) -# ser.write('#cmyM' + str(magn_y_max) + chr(13)) -# ser.write('#cmzm' + str(magn_z_min) + chr(13)) -# ser.write('#cmzM' + str(magn_z_max) + chr(13)) -# else: -# ser.write('#ccx' + str(magn_ellipsoid_center[0]) + chr(13)) -# ser.write('#ccy' + str(magn_ellipsoid_center[1]) + chr(13)) -# ser.write('#ccz' + str(magn_ellipsoid_center[2]) + chr(13)) -# ser.write('#ctxX' + str(magn_ellipsoid_transform[0][0]) + chr(13)) -# ser.write('#ctxY' + str(magn_ellipsoid_transform[0][1]) + chr(13)) -# ser.write('#ctxZ' + str(magn_ellipsoid_transform[0][2]) + chr(13)) -# ser.write('#ctyX' + str(magn_ellipsoid_transform[1][0]) + chr(13)) -# ser.write('#ctyY' + str(magn_ellipsoid_transform[1][1]) + chr(13)) -# ser.write('#ctyZ' + str(magn_ellipsoid_transform[1][2]) + chr(13)) -# ser.write('#ctzX' + str(magn_ellipsoid_transform[2][0]) + chr(13)) -# ser.write('#ctzY' + str(magn_ellipsoid_transform[2][1]) + chr(13)) -# ser.write('#ctzZ' + str(magn_ellipsoid_transform[2][2]) + chr(13)) - -# ser.write('#cgx' + str(gyro_average_offset_x) + chr(13)) -# ser.write('#cgy' + str(gyro_average_offset_y) + chr(13)) -# ser.write('#cgz' + str(gyro_average_offset_z) + chr(13)) + # #print calibration values for verification by user # ser.flushInput() From 726ee5dcb3f981e24d2e9191564218d29d26688b Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Mon, 21 Oct 2019 07:39:02 +0200 Subject: [PATCH 04/18] Remove parameters for disabling verification check --- nodes/imu_node.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index 6cc5a9b..f120bd3 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -191,14 +191,6 @@ def write_and_check_config(serial_instance, calib_dict): imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0) -# variables for calibration verification -verify_calibration = rospy.get_param('~verify_calibration', True) -calibration_file = rospy.get_param('~calibration_path', None) #set this in the launch file - -if verify_calibration and calibration_file is None: - rospy.logerr("Verify calibration has been enabled but no calibration path was provided") - exit(-1) - #rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max) #rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max) #rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(magn_ellipsoid_center), str(magn_ellipsoid_transform[0][0])) From 8e405d94e8fa1ebf24abc115f5be504de26fc9bf Mon Sep 17 00:00:00 2001 From: Pepe Date: Tue, 29 Oct 2019 10:51:01 +0000 Subject: [PATCH 05/18] Fixed the 'cold boot' issue with 14001 razor board --- src/Razor_AHRS/Razor_AHRS.ino | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/Razor_AHRS/Razor_AHRS.ino b/src/Razor_AHRS/Razor_AHRS.ino index 13b113c..2c98ec0 100644 --- a/src/Razor_AHRS/Razor_AHRS.ino +++ b/src/Razor_AHRS/Razor_AHRS.ino @@ -629,12 +629,25 @@ char readChar() void setup() { - // Init serial output - LOG_PORT.begin(OUTPUT__BAUD_RATE); - // Init status LED pinMode (STATUS_LED_PIN, OUTPUT); digitalWrite(STATUS_LED_PIN, LOW); + double time_old = 0; + // Init serial output + LOG_PORT.end(); + delay(50); + LOG_PORT.begin(OUTPUT__BAUD_RATE); + while(!LOG_PORT){ + if (millis() - time_old > 10000){ + time_old = millis(); + digitalWrite(STATUS_LED_PIN, HIGH); + delay(50); + #if HW__VERSION_CODE == 14001 + NVIC_SystemReset(); // processor software reset + #endif + } + } + // Init sensors delay(50); // Give sensors enough time to start @@ -672,6 +685,9 @@ void loop() { // Read incoming control messages #if HW__VERSION_CODE == 14001 + if (!LOG_PORT) + NVIC_SystemReset(); // processor software reset + // Compatibility fix : if bytes are sent 1 by 1 without being read, available() might never return more than 1... // Therefore, we need to read bytes 1 by 1 and the command byte needs to be a blocking read... if (LOG_PORT.available() >= 1) From 68332aecbb8d94b039c360f92586cffb3b0ca8bf Mon Sep 17 00:00:00 2001 From: Pepe Date: Tue, 29 Oct 2019 11:58:43 +0000 Subject: [PATCH 06/18] Added an option to ask for the magnetometer information --- src/Razor_AHRS/Output.ino | 22 +++++++++++++++++++++- src/Razor_AHRS/Razor_AHRS.ino | 32 +++++++++++++++++++++++++++++++- 2 files changed, 52 insertions(+), 2 deletions(-) diff --git a/src/Razor_AHRS/Output.ino b/src/Razor_AHRS/Output.ino index fe1291b..7f18459 100644 --- a/src/Razor_AHRS/Output.ino +++ b/src/Razor_AHRS/Output.ino @@ -103,6 +103,27 @@ void output_both_angles_and_sensors_text() LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.println(); } +void output_both_angles_and_all_sensors_text() +{ + LOG_PORT.print("#YPRAGM="); + LOG_PORT.print(TO_DEG(yaw)); LOG_PORT.print(","); + LOG_PORT.print(TO_DEG(pitch)); LOG_PORT.print(","); + LOG_PORT.print(TO_DEG(roll)); LOG_PORT.print(","); + + LOG_PORT.print(Accel_Vector[0]); LOG_PORT.print(","); + LOG_PORT.print(Accel_Vector[1]); LOG_PORT.print(","); + LOG_PORT.print(Accel_Vector[2]); LOG_PORT.print(","); + + LOG_PORT.print(Gyro_Vector[0]); LOG_PORT.print(","); + LOG_PORT.print(Gyro_Vector[1]); LOG_PORT.print(","); + LOG_PORT.print(Gyro_Vector[2]); LOG_PORT.print(","); + + //TODO:Check units and calibration + LOG_PORT.print(magnetom[0]); LOG_PORT.print(","); + LOG_PORT.print(magnetom[1]); LOG_PORT.print(","); + LOG_PORT.print(magnetom[2]); LOG_PORT.println(); +} + void output_sensors_binary() { LOG_PORT.write((byte*) accel, 12); @@ -145,4 +166,3 @@ void output_sensors() } } } - diff --git a/src/Razor_AHRS/Razor_AHRS.ino b/src/Razor_AHRS/Razor_AHRS.ino index 2c98ec0..52a381b 100644 --- a/src/Razor_AHRS/Razor_AHRS.ino +++ b/src/Razor_AHRS/Razor_AHRS.ino @@ -143,6 +143,12 @@ velocity is in rad/s. (Output frames have form like "#YPRAG=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01", followed by carriage return and line feed [\r\n]). + "#om" - Output angles and linear acceleration, rotational + velocity and magnetic field. Angles are in degrees, acceleration is + in units of 1.0 = 1/256 G (9.8/256 m/s^2). Rotational + velocity is in rad/s. (Output frames have form like + "#YPRAGM=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01,1.0,2.0,3.0", + followed by carriage return and line feed [\r\n]). // Sensor calibration "#oc" - Go to CALIBRATION output mode. @@ -208,7 +214,7 @@ // Select your hardware here by uncommenting one line! //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer) -//#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" +#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer) @@ -238,6 +244,7 @@ #define OUTPUT__MODE_SENSORS_RAW 3 // Outputs raw (uncalibrated) sensor values for all 9 axes #define OUTPUT__MODE_SENSORS_BOTH 4 // Outputs calibrated AND raw sensor values for all 9 axes #define OUTPUT__MODE_ANGLES_AG_SENSORS 5 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel +#define OUTPUT__MODE_ANGLES_AGM_SENSORS 6 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel + magnetometer data // Output format definitions (do not change) #define OUTPUT__FORMAT_TEXT 0 // Outputs data as text #define OUTPUT__FORMAT_BINARY 1 // Outputs data as binary float @@ -744,6 +751,11 @@ void loop() output_mode = OUTPUT__MODE_ANGLES_AG_SENSORS; output_format = OUTPUT__FORMAT_TEXT; } + else if (output_param == 'm') // Output angles + accel + rot. vel as te_x_t + { + output_mode = OUTPUT__MODE_ANGLES_AGM_SENSORS; + output_format = OUTPUT__FORMAT_TEXT; + } else if (output_param == 's') // Output _s_ensor values { char values_param = readChar(); @@ -1029,6 +1041,24 @@ void loop() if (output_stream_on || output_single_on) output_both_angles_and_sensors_text(); } + else if (output_mode == OUTPUT__MODE_ANGLES_AGM_SENSORS) // Output angles + accel + rot. vel + { + // Apply sensor calibration + compensate_sensor_errors(); + +#if DEBUG__USE_ONLY_DMP_M0 == true + Euler_angles_only_DMP_M0(); +#else + // Run DCM algorithm + Compass_Heading(); // Calculate magnetic heading + Matrix_update(); + Normalize(); + Drift_correction(); + Euler_angles(); +#endif // DEBUG__USE_ONLY_DMP_M0 + + if (output_stream_on || output_single_on) output_both_angles_and_all_sensors_text(); + } else // Output sensor values { if (output_stream_on || output_single_on) output_sensors(); From 33a6940eba9bcb94dc3894b8c0acc9d9a7d23297 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Wed, 30 Oct 2019 14:06:22 +0100 Subject: [PATCH 07/18] Publish IMU data, add some settings as parameters --- config/razor.yaml | 23 ++++-- nodes/imu_node.py | 207 ++++++++++++++++------------------------------ 2 files changed, 84 insertions(+), 146 deletions(-) diff --git a/config/razor.yaml b/config/razor.yaml index adb7da7..60ad330 100644 --- a/config/razor.yaml +++ b/config/razor.yaml @@ -1,9 +1,6 @@ -## USB port port: /dev/ttyUSB0 +frame_header: "base_imu_link" - -##### Calibration #### -### accelerometer accel_x_min: -250.0 accel_x_max: 250.0 accel_y_min: -250.0 @@ -11,8 +8,6 @@ accel_y_max: 250.0 accel_z_min: -250.0 accel_z_max: 250.0 -### magnetometer -# standard calibration magn_x_min: -600.0 magn_x_max: 600.0 magn_y_min: -600.0 @@ -20,7 +15,7 @@ magn_y_max: 600.0 magn_z_min: -600.0 magn_z_max: 600.0 -# extended calibration +# extended mag calibration calibration_magn_use_extended: false magn_ellipsoid_center: [0, 0, 0] magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]] @@ -28,7 +23,19 @@ magn_ellipsoid_transform: [[0, 0, 0], [0, 0, 0], [0, 0, 0]] # AHRS to robot calibration imu_yaw_calibration: 0.0 -### gyroscope gyro_average_offset_x: 0.0 gyro_average_offset_y: 0.0 gyro_average_offset_z: 0.0 + +# covariance +orientation_covariance: [0.0025, 0, 0, + 0, 0.0025, 0, + 0, 0, 0.0025] + +velocity_covariance: [0.002, 0, 0, + 0, 0.002, 0, + 0, 0, 0.002] + +acceleration_covariance: [0.04, 0, 0, + 0, 0.04, 0, + 0, 0, 0.04] diff --git a/nodes/imu_node.py b/nodes/imu_node.py index f120bd3..a991647 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -121,42 +121,10 @@ def write_and_check_config(serial_instance, calib_dict): 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 -# nonlinearity spec: 0.2% of full scale => 8 degrees/sec = 0.14 rad/sec -# Choosing the larger (0.14) as std dev, variance = 0.14^2 ~= 0.02 -imuMsg.angular_velocity_covariance = [ -0.02, 0 , 0, -0 , 0.02, 0, -0 , 0 , 0.02 -] - -# linear acceleration covariance estimation: -# observed acceleration noise: 5 counts => 20milli-G's ~= 0.2m/s^2 -# nonliniarity spec: 0.5% of full scale => 0.2m/s^2 -# Choosing 0.2 as std dev, variance = 0.2^2 = 0.04 -imuMsg.linear_acceleration_covariance = [ -0.04 , 0 , 0, -0 , 0.04, 0, -0 , 0 , 0.04 -] +imuMsg.orientation_covariance = rospy.get_param('~orientation_covariance') +imuMsg.angular_velocity_covariance = rospy.get_param('~velocity_covariance') +imuMsg.linear_acceleration_covariance = rospy.get_param('~acceleration_covariance') +imuMsg.header.frame_id = rospy.get_param('~frame_header', 'base_imu_link') default_port='/dev/ttyUSB0' port = rospy.get_param('~port', default_port) @@ -191,11 +159,6 @@ def write_and_check_config(serial_instance, calib_dict): imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0) -#rospy.loginfo("%f %f %f %f %f %f", accel_x_min, accel_x_max, accel_y_min, accel_y_max, accel_z_min, accel_z_max) -#rospy.loginfo("%f %f %f %f %f %f", magn_x_min, magn_x_max, magn_y_min, magn_y_max, magn_z_min, magn_z_max) -#rospy.loginfo("%s %s %s", str(calibration_magn_use_extended), str(magn_ellipsoid_center), str(magn_ellipsoid_transform[0][0])) -#rospy.loginfo("%f %f %f", gyro_average_offset_x, gyro_average_offset_y, gyro_average_offset_z) - # Check your COM port and baud rate rospy.loginfo("Opening %s...", port) try: @@ -217,101 +180,69 @@ def write_and_check_config(serial_instance, calib_dict): #stop datastream send_command(ser, STOP_DATASTREAM) write_and_check_config(ser, calib_dict) - -#discard old input -#automatic flush - NOT WORKING -#ser.flushInput() #discard old input, still in invalid format -# #flush manually, as above command is not working -# discard = ser.readlines() - -# #set output mode -# ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text - -# rospy.loginfo("Writing calibration values to razor IMU board...") -# #set calibration values - - -# #print calibration values for verification by user -# ser.flushInput() -# ser.write('#p' + chr(13)) -# calib_data = ser.readlines() -# calib_data_print = "Printing set calibration values:\r\n" -# for line in calib_data: -# calib_data_print += line -# rospy.loginfo(calib_data_print) - -# #start datastream -# ser.write('#o1' + chr(13)) - -# #automatic flush - NOT WORKING -# #ser.flushInput() #discard old input, still in invalid format -# #flush manually, as above command is not working - it breaks the serial connection -# rospy.loginfo("Flushing first 200 IMU entries...") -# for x in range(0, 200): -# line = ser.readline() -# rospy.loginfo("Publishing IMU data...") -# #f = open("raw_imu_data.log", 'w') - -# while not rospy.is_shutdown(): -# line = ser.readline() -# line = line.replace("#YPRAG=","") # Delete "#YPRAG=" -# #f.write(line) # Write to the output log file -# words = string.split(line,",") # Fields split -# if len(words) > 2: -# #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) -# yaw_deg = -float(words[0]) -# yaw_deg = yaw_deg + imu_yaw_calibration -# if yaw_deg > 180.0: -# yaw_deg = yaw_deg - 360.0 -# if yaw_deg < -180.0: -# yaw_deg = yaw_deg + 360.0 -# yaw = yaw_deg*degrees2rad -# #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) -# pitch = -float(words[1])*degrees2rad -# roll = float(words[2])*degrees2rad - -# # Publish message -# # AHRS firmware accelerations are negated -# # This means y and z are correct for ROS, but x needs reversing -# imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor -# imuMsg.linear_acceleration.y = float(words[4]) * accel_factor -# imuMsg.linear_acceleration.z = float(words[5]) * accel_factor - -# imuMsg.angular_velocity.x = float(words[6]) -# #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) -# imuMsg.angular_velocity.y = -float(words[7]) -# #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) -# imuMsg.angular_velocity.z = -float(words[8]) - -# q = quaternion_from_euler(roll,pitch,yaw) -# imuMsg.orientation.x = q[0] -# imuMsg.orientation.y = q[1] -# imuMsg.orientation.z = q[2] -# imuMsg.orientation.w = q[3] -# imuMsg.header.stamp= rospy.Time.now() -# imuMsg.header.frame_id = 'base_imu_link' -# imuMsg.header.seq = seq -# seq = seq + 1 -# pub.publish(imuMsg) - -# if (diag_pub_time < rospy.get_time()) : -# diag_pub_time += 1 -# diag_arr = DiagnosticArray() -# diag_arr.header.stamp = rospy.get_rostime() -# diag_arr.header.frame_id = '1' -# diag_msg = DiagnosticStatus() -# diag_msg.name = 'Razor_Imu' -# diag_msg.level = DiagnosticStatus.OK -# diag_msg.message = 'Received AHRS measurement' -# diag_msg.values.append(KeyValue('roll (deg)', -# str(roll*(180.0/math.pi)))) -# diag_msg.values.append(KeyValue('pitch (deg)', -# str(pitch*(180.0/math.pi)))) -# diag_msg.values.append(KeyValue('yaw (deg)', -# str(yaw*(180.0/math.pi)))) -# diag_msg.values.append(KeyValue('sequence number', str(seq))) -# diag_arr.status.append(diag_msg) -# diag_pub.publish(diag_arr) +send_command(ser, SET_TEXT_EXTENDED_FORMAT) +send_command(ser, START_DATASTREAM) + +while not rospy.is_shutdown(): + line = ser.readline() + if not line.startswith("#YPRAG="): + rospy.logerr_throttle(1, "Did not find #YPRAG in the received IMU message") + continue + line = line.replace("#YPRAG=","") # Delete "#YPRAG=" + words = string.split(line,",") # Fields split + if len(words) > 2: + #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) + yaw_deg = -float(words[0]) + yaw_deg = yaw_deg + imu_yaw_calibration + if yaw_deg > 180.0: + yaw_deg = yaw_deg - 360.0 + if yaw_deg < -180.0: + yaw_deg = yaw_deg + 360.0 + yaw = yaw_deg*degrees2rad + #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) + pitch = -float(words[1])*degrees2rad + roll = float(words[2])*degrees2rad + + # Publish message + # AHRS firmware accelerations are negated + # This means y and z are correct for ROS, but x needs reversing + imuMsg.linear_acceleration.x = -float(words[3]) * accel_factor + imuMsg.linear_acceleration.y = float(words[4]) * accel_factor + imuMsg.linear_acceleration.z = float(words[5]) * accel_factor + + imuMsg.angular_velocity.x = float(words[6]) + #in AHRS firmware y axis points right, in ROS y axis points left (see REP 103) + imuMsg.angular_velocity.y = -float(words[7]) + #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) + imuMsg.angular_velocity.z = -float(words[8]) + + q = quaternion_from_euler(roll,pitch,yaw) + imuMsg.orientation.x = q[0] + imuMsg.orientation.y = q[1] + imuMsg.orientation.z = q[2] + imuMsg.orientation.w = q[3] + imuMsg.header.stamp= rospy.Time.now() + imuMsg.header.seq = seq + seq = seq + 1 + pub.publish(imuMsg) + + if (diag_pub_time < rospy.get_time()) : + diag_pub_time += 1 + diag_arr = DiagnosticArray() + diag_arr.header.stamp = rospy.get_rostime() + diag_arr.header.frame_id = '1' + diag_msg = DiagnosticStatus() + diag_msg.name = 'Razor_Imu' + diag_msg.level = DiagnosticStatus.OK + diag_msg.message = 'Received AHRS measurement' + diag_msg.values.append(KeyValue('roll (deg)', + str(roll*(180.0/math.pi)))) + diag_msg.values.append(KeyValue('pitch (deg)', + str(pitch*(180.0/math.pi)))) + diag_msg.values.append(KeyValue('yaw (deg)', + str(yaw*(180.0/math.pi)))) + diag_msg.values.append(KeyValue('sequence number', str(seq))) + diag_arr.status.append(diag_msg) + diag_pub.publish(diag_arr) -# ser.close -# #f.close +ser.close From c86131106cf7c908a6306562617ed08bc8db1956 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Wed, 30 Oct 2019 14:15:52 +0100 Subject: [PATCH 08/18] Comment out HW__VERSION_CODE in .ino file --- src/Razor_AHRS/Razor_AHRS.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Razor_AHRS/Razor_AHRS.ino b/src/Razor_AHRS/Razor_AHRS.ino index f794576..37fc373 100644 --- a/src/Razor_AHRS/Razor_AHRS.ino +++ b/src/Razor_AHRS/Razor_AHRS.ino @@ -208,7 +208,7 @@ // Select your hardware here by uncommenting one line! //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer) -#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" +// #define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer) From 2992ee7e9d3188162b114ba3ed39add2f44ddc6e Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Wed, 30 Oct 2019 14:17:03 +0100 Subject: [PATCH 09/18] Fix whitespace in .ino comment --- src/Razor_AHRS/Razor_AHRS.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Razor_AHRS/Razor_AHRS.ino b/src/Razor_AHRS/Razor_AHRS.ino index 37fc373..13b113c 100644 --- a/src/Razor_AHRS/Razor_AHRS.ino +++ b/src/Razor_AHRS/Razor_AHRS.ino @@ -208,7 +208,7 @@ // Select your hardware here by uncommenting one line! //#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer) -// #define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" +//#define HW__VERSION_CODE 14001 // SparkFun "9DoF Razor IMU M0" version "SEN-14001" //#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer) //#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer) From 61c30cfa3f1230cb742fd5cfd5b8e68836ebc6d8 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Wed, 30 Oct 2019 14:21:53 +0100 Subject: [PATCH 10/18] Add empty line at the end of serial_commands.py file --- nodes/lib/serial_commands.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nodes/lib/serial_commands.py b/nodes/lib/serial_commands.py index c20bac5..6b7837f 100644 --- a/nodes/lib/serial_commands.py +++ b/nodes/lib/serial_commands.py @@ -44,4 +44,4 @@ SET_GYRO_AVERAGE_OFFSET_X = '#cgx' SET_GYRO_AVERAGE_OFFSET_Y = '#cgy' -SET_GYRO_AVERAGE_OFFSET_Z = '#cgz' \ No newline at end of file +SET_GYRO_AVERAGE_OFFSET_Z = '#cgz' From 7daf03b7f26f7dec7499f4868e7a0ba65318f45f Mon Sep 17 00:00:00 2001 From: Pepe Date: Thu, 31 Oct 2019 10:24:27 +0000 Subject: [PATCH 11/18] Added the magnetometer serial command and replaced the line start from a string to a variable --- config/razor.yaml | 4 ++++ nodes/imu_node.py | 8 ++++---- nodes/lib/serial_commands.py | 7 +++++-- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/config/razor.yaml b/config/razor.yaml index 60ad330..eb3adce 100644 --- a/config/razor.yaml +++ b/config/razor.yaml @@ -39,3 +39,7 @@ velocity_covariance: [0.002, 0, 0, acceleration_covariance: [0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04] + +magnetometer_covariance: [0.00, 0, 0, + 0, 0.00, 0, + 0, 0, 0.00] diff --git a/nodes/imu_node.py b/nodes/imu_node.py index a991647..d58b1dd 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -55,7 +55,7 @@ def reconfig_callback(config, level): rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration)) return config -def send_command(serial_instance, command, value=None): +def send_command(serial_instance, command, value = None): if value is None: cmd = command + chr(13) else: @@ -180,15 +180,15 @@ def write_and_check_config(serial_instance, calib_dict): #stop datastream send_command(ser, STOP_DATASTREAM) write_and_check_config(ser, calib_dict) -send_command(ser, SET_TEXT_EXTENDED_FORMAT) +send_command(ser, SET_TEXT_EXTENDED_FORMAT_NO_MAG) send_command(ser, START_DATASTREAM) while not rospy.is_shutdown(): line = ser.readline() - if not line.startswith("#YPRAG="): + if not line.startswith(LINE_START_NO_MAG): rospy.logerr_throttle(1, "Did not find #YPRAG in the received IMU message") continue - line = line.replace("#YPRAG=","") # Delete "#YPRAG=" + line = line.replace(LINE_START_NO_MAG,"") # Delete "#YPRAG=" words = string.split(line,",") # Fields split if len(words) > 2: #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) diff --git a/nodes/lib/serial_commands.py b/nodes/lib/serial_commands.py index 6b7837f..b979ae2 100644 --- a/nodes/lib/serial_commands.py +++ b/nodes/lib/serial_commands.py @@ -5,11 +5,14 @@ SET_BINARY_FORMAT = '#ob' SET_TEXT_FORMAT = '#ot' -SET_TEXT_EXTENDED_FORMAT = '#ox' # Use this one in the ROS driver - +SET_TEXT_EXTENDED_FORMAT_NO_MAG = '#ox' # Use this one in the ROS driver WITHOUT magnetometer +SET_TEXT_EXTENDED_FORMAT_WITH_MAG = '#om' # Use this one in the ROS driver WITH magnetometer SET_CALIBRATION_OUTPUT_MODE = '#oc' NEXT_SENSOR_CALIBRATION = '#on' +LINE_START_NO_MAG = "#YPRAG=" +LINE_START_WITH_MAG = "#YPRAGM=" + ENABLE_ERROR_MESSAGE = '#oe1' DISABLE_ERROR_MESSAGE = '#oe0' OUTPUT_ERROR_COUNT = '#oec' From 08488e05cdbbbf5e382c7cade5c94405efa5decc Mon Sep 17 00:00:00 2001 From: Pepe Date: Thu, 31 Oct 2019 11:43:12 +0000 Subject: [PATCH 12/18] added the option to use the magnetometer --- config/razor.yaml | 1 + nodes/imu_node.py | 19 ++++++++++++++----- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/config/razor.yaml b/config/razor.yaml index eb3adce..b48af48 100644 --- a/config/razor.yaml +++ b/config/razor.yaml @@ -1,5 +1,6 @@ port: /dev/ttyUSB0 frame_header: "base_imu_link" +use_magnetometer: false accel_x_min: -250.0 accel_x_max: 250.0 diff --git a/nodes/imu_node.py b/nodes/imu_node.py index d58b1dd..789878d 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -113,7 +113,7 @@ def write_and_check_config(serial_instance, calib_dict): rospy.logwarn("The calibration value of [%s] did not match. Expected: %s, received: %s", key, str(calib_dict[key]), str(config_parsed[key])) -rospy.init_node("razor_node") +rospy.init_node("razor_imu") #We only care about the most recent measurement, i.e. queue_size=1 pub = rospy.Publisher('imu', Imu, queue_size=1) srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback @@ -126,6 +126,7 @@ def write_and_check_config(serial_instance, calib_dict): imuMsg.linear_acceleration_covariance = rospy.get_param('~acceleration_covariance') imuMsg.header.frame_id = rospy.get_param('~frame_header', 'base_imu_link') + default_port='/dev/ttyUSB0' port = rospy.get_param('~port', default_port) @@ -180,15 +181,23 @@ def write_and_check_config(serial_instance, calib_dict): #stop datastream send_command(ser, STOP_DATASTREAM) write_and_check_config(ser, calib_dict) -send_command(ser, SET_TEXT_EXTENDED_FORMAT_NO_MAG) +publish_magnetometer = rospy.get_param('~use_magnetometer', False) + +if publish_magnetometer: + send_command(ser, SET_TEXT_EXTENDED_FORMAT_NO_MAG) + line_start = LINE_START_NO_MAG +else: + send_command(ser, SET_TEXT_EXTENDED_FORMAT_WITH_MAG) + line_start = LINE_START_WITH_MAG + send_command(ser, START_DATASTREAM) while not rospy.is_shutdown(): line = ser.readline() - if not line.startswith(LINE_START_NO_MAG): - rospy.logerr_throttle(1, "Did not find #YPRAG in the received IMU message") + if not line.startswith(line_start): + rospy.logerr_throttle(1, "Did not find correct line start in the received IMU message") continue - line = line.replace(LINE_START_NO_MAG,"") # Delete "#YPRAG=" + line = line.replace(line_start,"") # Delete "#YPRAG=" words = string.split(line,",") # Fields split if len(words) > 2: #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) From 261915663ec8a9e3efbab34bb354848a1dc237ea Mon Sep 17 00:00:00 2001 From: Pepe Date: Thu, 31 Oct 2019 11:56:48 +0000 Subject: [PATCH 13/18] added magnetometer optional publisher --- config/razor.yaml | 8 ++++---- nodes/imu_node.py | 32 +++++++++++++++++++++++++------- 2 files changed, 29 insertions(+), 11 deletions(-) diff --git a/config/razor.yaml b/config/razor.yaml index b48af48..80d253b 100644 --- a/config/razor.yaml +++ b/config/razor.yaml @@ -1,6 +1,6 @@ port: /dev/ttyUSB0 frame_header: "base_imu_link" -use_magnetometer: false +publish_magnetometer: false accel_x_min: -250.0 accel_x_max: 250.0 @@ -41,6 +41,6 @@ acceleration_covariance: [0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04] -magnetometer_covariance: [0.00, 0, 0, - 0, 0.00, 0, - 0, 0, 0.00] +magnetic_field_covariance: [0.00, 0, 0, + 0, 0.00, 0, + 0, 0, 0.00] diff --git a/nodes/imu_node.py b/nodes/imu_node.py index 789878d..37bdce3 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -38,6 +38,7 @@ #from time import time from sensor_msgs.msg import Imu +from sensor_msgs.msg import MagneticField from tf.transformations import quaternion_from_euler from dynamic_reconfigure.server import Server from razor_imu_9dof.cfg import imuConfig @@ -113,9 +114,11 @@ def write_and_check_config(serial_instance, calib_dict): rospy.logwarn("The calibration value of [%s] did not match. Expected: %s, received: %s", key, str(calib_dict[key]), str(config_parsed[key])) +publish_magnetometer = rospy.get_param('~publish_magnetometer', False) + rospy.init_node("razor_imu") #We only care about the most recent measurement, i.e. queue_size=1 -pub = rospy.Publisher('imu', Imu, queue_size=1) +pub_imu = rospy.Publisher('imu', 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() @@ -126,6 +129,12 @@ def write_and_check_config(serial_instance, calib_dict): imuMsg.linear_acceleration_covariance = rospy.get_param('~acceleration_covariance') imuMsg.header.frame_id = rospy.get_param('~frame_header', 'base_imu_link') +if publish_magnetometer: + pub_mag = rospy.Publisher('mag', MagneticField, queue_size=1) + magMsg = MagneticField() + magMsg.magnetic_field_covariance = rospy.get_param('~magnetic_field_covariance') + magMsg.header.frame_id = rospy.get_param('~frame_header', 'base_imu_link') + #should a separate diagnostic for the Magnetometer should be done? default_port='/dev/ttyUSB0' port = rospy.get_param('~port', default_port) @@ -181,14 +190,13 @@ def write_and_check_config(serial_instance, calib_dict): #stop datastream send_command(ser, STOP_DATASTREAM) write_and_check_config(ser, calib_dict) -publish_magnetometer = rospy.get_param('~use_magnetometer', False) if publish_magnetometer: - send_command(ser, SET_TEXT_EXTENDED_FORMAT_NO_MAG) - line_start = LINE_START_NO_MAG -else: send_command(ser, SET_TEXT_EXTENDED_FORMAT_WITH_MAG) line_start = LINE_START_WITH_MAG +else: + send_command(ser, SET_TEXT_EXTENDED_FORMAT_NO_MAG) + line_start = LINE_START_NO_MAG send_command(ser, START_DATASTREAM) @@ -197,7 +205,7 @@ def write_and_check_config(serial_instance, calib_dict): if not line.startswith(line_start): rospy.logerr_throttle(1, "Did not find correct line start in the received IMU message") continue - line = line.replace(line_start,"") # Delete "#YPRAG=" + line = line.replace(line_start,"") # Delete "#YPRAG=" or "#YPRAGM=" words = string.split(line,",") # Fields split if len(words) > 2: #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) @@ -225,6 +233,11 @@ def write_and_check_config(serial_instance, calib_dict): #in AHRS firmware z axis points down, in ROS z axis points up (see REP 103) imuMsg.angular_velocity.z = -float(words[8]) + if publish_magnetometer: + magMsg.magnetic_field.x = float(words[9]) + magMsg.magnetic_field.y = float(words[10]) + magMsg.magnetic_field.z = float(words[11]) + q = quaternion_from_euler(roll,pitch,yaw) imuMsg.orientation.x = q[0] imuMsg.orientation.y = q[1] @@ -233,7 +246,12 @@ def write_and_check_config(serial_instance, calib_dict): imuMsg.header.stamp= rospy.Time.now() imuMsg.header.seq = seq seq = seq + 1 - pub.publish(imuMsg) + pub_imu.publish(imuMsg) + + if publish_magnetometer: + magMsg.header.stamp = imuMsg.header.stamp + magMsg.header.seq = imuMsg.header.seq + pub_mag.publish(magMsg) if (diag_pub_time < rospy.get_time()) : diag_pub_time += 1 From 4a7c7839d5e4f6a0da8d81c812773623872c6336 Mon Sep 17 00:00:00 2001 From: Pepe Date: Thu, 31 Oct 2019 12:27:48 +0000 Subject: [PATCH 14/18] added the correct units and axis of the mag according to REP103 --- nodes/imu_node.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index 37bdce3..234fcca 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -234,9 +234,15 @@ def write_and_check_config(serial_instance, calib_dict): imuMsg.angular_velocity.z = -float(words[8]) if publish_magnetometer: - magMsg.magnetic_field.x = float(words[9]) - magMsg.magnetic_field.y = float(words[10]) - magMsg.magnetic_field.z = float(words[11]) + #according to line 178 the units published of the mag are mGauss + #REP103 specifys the units of magnetic field to be Teslas + #The axis of the MPU magnetometer are X forward, Y right and Z down, + # but Sparkfun the firmware interchanges x and y and changes the sign of y + # so to get it to REP103 we need to swap X and Y again and make Y and Z negative + magMsg.magnetic_field.x = float(words[10]) * 1e-7 + magMsg.magnetic_field.y = -float(words[9]) * 1e-7 + magMsg.magnetic_field.z = -float(words[11]) * 1e-7 + #check frame orientation and units q = quaternion_from_euler(roll,pitch,yaw) imuMsg.orientation.x = q[0] @@ -247,7 +253,7 @@ def write_and_check_config(serial_instance, calib_dict): imuMsg.header.seq = seq seq = seq + 1 pub_imu.publish(imuMsg) - + if publish_magnetometer: magMsg.header.stamp = imuMsg.header.stamp magMsg.header.seq = imuMsg.header.seq From 7e7686a613558b9e2183a94cbf8012503f63b1d2 Mon Sep 17 00:00:00 2001 From: Pepe Date: Thu, 31 Oct 2019 14:29:15 +0000 Subject: [PATCH 15/18] specific changes for the magnetometer yaml and config filein the launch --- config/razor.yaml | 6 ++++-- launch/razor-pub.launch | 2 +- nodes/imu_node.py | 34 +++++++++++++++++++++------------- 3 files changed, 26 insertions(+), 16 deletions(-) diff --git a/config/razor.yaml b/config/razor.yaml index 80d253b..e4e15dc 100644 --- a/config/razor.yaml +++ b/config/razor.yaml @@ -1,6 +1,6 @@ -port: /dev/ttyUSB0 +port: /dev/ttyACM0 frame_header: "base_imu_link" -publish_magnetometer: false + accel_x_min: -250.0 accel_x_max: 250.0 @@ -16,6 +16,8 @@ magn_y_max: 600.0 magn_z_min: -600.0 magn_z_max: 600.0 +publish_magnetometer: true + # extended mag calibration calibration_magn_use_extended: false magn_ellipsoid_center: [0, 0, 0] diff --git a/launch/razor-pub.launch b/launch/razor-pub.launch index 7eb97d1..ec369b7 100755 --- a/launch/razor-pub.launch +++ b/launch/razor-pub.launch @@ -1,5 +1,5 @@ - + diff --git a/nodes/imu_node.py b/nodes/imu_node.py index 234fcca..fe50722 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -35,6 +35,7 @@ from time import sleep from lib.serial_commands import * import yaml +import time #from time import time from sensor_msgs.msg import Imu @@ -61,11 +62,11 @@ def send_command(serial_instance, command, value = None): cmd = command + chr(13) else: cmd = command + str(value) + chr(13) - rospy.loginfo("Sending: %s", cmd) + rospy.loginfo("Razor IMU -> Sending: %s", cmd) expected_len = len(cmd) res = serial_instance.write(cmd) if (res != expected_len): - rospy.logerr("Expected serial command len (%d) didn't match amount of bytes written (%d) for command %s", expected_len, res, command) + rospy.logerr("Razor IMU -> Expected serial command len (%d) didn't match amount of bytes written (%d) for command %s", expected_len, res, command) sleep(0.05) # Don't spam serial with too many commands at once def write_and_check_config(serial_instance, calib_dict): @@ -114,8 +115,6 @@ def write_and_check_config(serial_instance, calib_dict): rospy.logwarn("The calibration value of [%s] did not match. Expected: %s, received: %s", key, str(calib_dict[key]), str(config_parsed[key])) -publish_magnetometer = rospy.get_param('~publish_magnetometer', False) - rospy.init_node("razor_imu") #We only care about the most recent measurement, i.e. queue_size=1 pub_imu = rospy.Publisher('imu', Imu, queue_size=1) @@ -129,6 +128,8 @@ def write_and_check_config(serial_instance, calib_dict): imuMsg.linear_acceleration_covariance = rospy.get_param('~acceleration_covariance') imuMsg.header.frame_id = rospy.get_param('~frame_header', 'base_imu_link') +publish_magnetometer = rospy.get_param('~publish_magnetometer', False) + if publish_magnetometer: pub_mag = rospy.Publisher('mag', MagneticField, queue_size=1) magMsg = MagneticField() @@ -170,21 +171,26 @@ def write_and_check_config(serial_instance, calib_dict): imu_yaw_calibration = rospy.get_param('~imu_yaw_calibration', 0.0) # Check your COM port and baud rate -rospy.loginfo("Opening %s...", port) -try: - ser = serial.Serial(port=port, baudrate=57600, timeout=1) -except serial.serialutil.SerialException: - rospy.logerr("IMU not found at port "+port + ". Did you specify the correct port in the launch file?") - #exit - sys.exit(0) +rospy.loginfo("Razor IMU -> Opening %s...", port) +connection_attempts = 5 +for connection_tries in range (0,connection_attempts + 1): + try: + ser = serial.Serial(port=port, baudrate=57600, timeout=1) + break + except serial.serialutil.SerialException: + rospy.logerr("Razor IMU not found at port " + port + ". Did you specify the correct port in the launch file? Trying %d more times...", 5 - connection_tries) + + if connection_tries == connection_attempts: + #exit + sys.exit(0) + time.sleep(3) roll=0 pitch=0 yaw=0 seq=0 accel_factor = 9.806 / 256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2. -# rospy.loginfo("Giving the razor IMU board 5 seconds to boot...") -# rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot + ### configure board ### #stop datastream @@ -200,6 +206,8 @@ def write_and_check_config(serial_instance, calib_dict): send_command(ser, START_DATASTREAM) +rospy.loginfo("Razor IMU up and running") + while not rospy.is_shutdown(): line = ser.readline() if not line.startswith(line_start): From 3431643168ec08d4981effa5b553d029c4ecfc50 Mon Sep 17 00:00:00 2001 From: Pepe Date: Thu, 31 Oct 2019 14:37:50 +0000 Subject: [PATCH 16/18] added magnetometer axis explanation --- nodes/imu_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index fe50722..a38603c 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -244,7 +244,8 @@ def write_and_check_config(serial_instance, calib_dict): if publish_magnetometer: #according to line 178 the units published of the mag are mGauss #REP103 specifys the units of magnetic field to be Teslas - #The axis of the MPU magnetometer are X forward, Y right and Z down, + #The axis of the MPU magnetometer are X forward, Y right and Z down + # when the chip is facing forward, in the sparkfun board, the chip is facing the left side # but Sparkfun the firmware interchanges x and y and changes the sign of y # so to get it to REP103 we need to swap X and Y again and make Y and Z negative magMsg.magnetic_field.x = float(words[10]) * 1e-7 From dd7ce43c500b0e0e0fa411f73f7bb3215bcaa004 Mon Sep 17 00:00:00 2001 From: Mateusz Sadowski Date: Wed, 6 Nov 2019 14:18:52 +0100 Subject: [PATCH 17/18] Fix magnetic field orientation --- nodes/imu_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index a38603c..f6d335e 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -248,8 +248,8 @@ def write_and_check_config(serial_instance, calib_dict): # when the chip is facing forward, in the sparkfun board, the chip is facing the left side # but Sparkfun the firmware interchanges x and y and changes the sign of y # so to get it to REP103 we need to swap X and Y again and make Y and Z negative - magMsg.magnetic_field.x = float(words[10]) * 1e-7 - magMsg.magnetic_field.y = -float(words[9]) * 1e-7 + magMsg.magnetic_field.x = float(words[9]) * 1e-7 + magMsg.magnetic_field.y = -float(words[10]) * 1e-7 magMsg.magnetic_field.z = -float(words[11]) * 1e-7 #check frame orientation and units From 412781d3411e9392d1ecf9cf632f19f73dc689a8 Mon Sep 17 00:00:00 2001 From: Pepe Date: Tue, 12 Nov 2019 09:55:14 +0000 Subject: [PATCH 18/18] Changed the magnetometer frame according to Mat --- nodes/imu_node.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/nodes/imu_node.py b/nodes/imu_node.py index a38603c..3a59616 100755 --- a/nodes/imu_node.py +++ b/nodes/imu_node.py @@ -247,9 +247,10 @@ def write_and_check_config(serial_instance, calib_dict): #The axis of the MPU magnetometer are X forward, Y right and Z down # when the chip is facing forward, in the sparkfun board, the chip is facing the left side # but Sparkfun the firmware interchanges x and y and changes the sign of y - # so to get it to REP103 we need to swap X and Y again and make Y and Z negative - magMsg.magnetic_field.x = float(words[10]) * 1e-7 - magMsg.magnetic_field.y = -float(words[9]) * 1e-7 + # so to get it to REP103 we need to make X and Z negative + #The magn frames form sparkfun can be seen in line 178 from Sensors.ino + magMsg.magnetic_field.x = -float(words[9]) * 1e-7 + magMsg.magnetic_field.y = float(words[10]) * 1e-7 magMsg.magnetic_field.z = -float(words[11]) * 1e-7 #check frame orientation and units