diff --git a/Makefile b/Makefile deleted file mode 100644 index b75b928..0000000 --- a/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/README b/README deleted file mode 100644 index 61cef0a..0000000 --- a/README +++ /dev/null @@ -1,21 +0,0 @@ -This ROS node will read messages published to the /imu/data and /imu/mag topics. For the moment it only performs magnetometer calibration. - -* To run the node - -rosrun calibrate_imu calibrate_imu.py - -* To start collecting samples from the IMU - -rosservice call /start_sampling - -* To calibrate the magnetometer using the current data samples - -rosservice call /calibrate_mag - -This will store the calibration parameters in a file called "mag_calibration_d_m_y_H_M_S", where d_m_y_H_M_S is the time the file was created. For an example on how to read the file you can look at the plot_calibration_data.py script - -* To visualize the raw data from the magnetometer and the calibrated data - -./plot_calibration_data.py mag_calibration_d_m_y_H_M_S - -If you have any questions or comments, please shoot me at email at gamboa at cim dot mcgill dot ca diff --git a/README.md b/README.md new file mode 100644 index 0000000..f0f0141 --- /dev/null +++ b/README.md @@ -0,0 +1,33 @@ +# calibrate_imu +This ROS node will read messages published to the /imu/data and /imu/mag topics. For the moment it only performs magnetometer calibration. + + +To run the node + +```shell +rosrun calibrate_imu calibrate_imu.py +``` + +To start collecting samples from the IMU + +```shell +rosservice call /start_sampling +``` + +To calibrate the magnetometer using the current data samples +```shell +rosservice call /calibrate_mag +``` + + + +If you have any questions or comments, please shoot me at email at gamboa at cim dot mcgill dot ca + +# Limitations +This branch does not save the resulting configuration, it is only printed to the terminal, you will need to perform the adjustment in another node such as the `imu_filter_madwick` from the `imu_tools` package. \ No newline at end of file diff --git a/nodes/calibrate_imu.py b/nodes/calibrate_imu.py index cd4933f..85e060f 100755 --- a/nodes/calibrate_imu.py +++ b/nodes/calibrate_imu.py @@ -1,7 +1,6 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import rospy -from std_msgs.msg import Header -from sensor_msgs.msg import Imu +from sensor_msgs.msg import Imu, MagneticField from geometry_msgs.msg import Vector3Stamped from std_srvs.srv import Empty, EmptyResponse import math @@ -21,13 +20,13 @@ def __init__(self): mag_topic = rospy.get_param('~mag_topic', '/imu/mag') imu_topic = rospy.get_param('~imu_topic', '/imu/data') - self.publish_calibrated = rospy.get_param("~publish_calibrated", False) + self.publish_calibrated = rospy.get_param("~publish_calibrated", True) self.max_samples = rospy.get_param("~max_samples", 5000) self.__location__ = rospy.get_param("~calibrations_dir", "/data/mag_calibration") - self.calibrated_mag_pub = rospy.Publisher('/imu/mag_calibrated',Vector3Stamped, queue_size='1') + self.calibrated_mag_pub = rospy.Publisher('/imu/mag_calibrated', MagneticField, queue_size=1) self.load_calibration() - rospy.Subscriber(mag_topic,Vector3Stamped,self.mag_cb) + rospy.Subscriber(mag_topic,MagneticField,self.mag_cb) rospy.Subscriber(imu_topic,Imu, self.imu_cb) rospy.Service('start_sampling',Empty,self.start_sampling) rospy.Service('stop_sampling',Empty,self.stop_sampling) @@ -42,7 +41,7 @@ def load_calibration(self): self.mag_samples = pickle.load(f) f.close() except: - rospy.log_error("Could not open file %s"%(file_path)) + rospy.logerr("Could not open file %s"%(file_path)) rospy.loginfo("Magnetometer offset:\n %s",self.mag_offset) rospy.loginfo("Magnetometer Calibration Matrix:\n %s",self.mag_matrix) @@ -51,7 +50,8 @@ def load_calibration(self): def mag_cb(self,data): if self.sampling == True: #collect measurements - self.mag_samples.append([data.vector.x,data.vector.y,data.vector.z]) + self.mag_samples.append([data.magnetic_field.x,data.magnetic_field.y,data.magnetic_field.z]) + if len(self.mag_samples)%50==0: rospy.loginfo("Got %d magnetometer readings"%(len(self.mag_samples))) if len(self.mag_samples) > self.max_samples: @@ -59,13 +59,13 @@ def mag_cb(self,data): self.mag_samples = self.mag_samples[50:] if self.publish_calibrated == True: - mag_raw = array([data.vector.x,data.vector.y,data.vector.z]) + mag_raw = array([data.magnetic_field.x,data.magnetic_field.y,data.magnetic_field.z]) mag_cal = ((self.mag_matrix.dot((mag_raw-self.mag_offset.T).T)).T) - calibrated_mag_msg = Vector3Stamped() + calibrated_mag_msg = MagneticField() calibrated_mag_msg.header.stamp = data.header.stamp - calibrated_mag_msg.vector.x = mag_cal[0] - calibrated_mag_msg.vector.y = mag_cal[1] - calibrated_mag_msg.vector.z = mag_cal[2] + calibrated_mag_msg.magnetic_field.x = mag_cal[0] + calibrated_mag_msg.magnetic_field.y = mag_cal[1] + calibrated_mag_msg.magnetic_field.z = mag_cal[2] self.calibrated_mag_pub.publish(calibrated_mag_msg) @@ -132,23 +132,27 @@ def calibrate_mag(self,req): L = eye(3) try: L = linalg.cholesky(Q).transpose() - except Exception,e: + except Exception as e: rospy.loginfo(str(e)) L = eye(3) rospy.loginfo("Magnetometer offset:\n %s",x0) rospy.loginfo("Magnetometer Calibration Matrix:\n %s",L) - file_path = os.path.join(self.__location__,"last_mag_calibration") - f = open(file_path,"w+") - pickle.dump(x0,f) - pickle.dump(L,f) - pickle.dump(matrix(self.mag_samples),f) - f.close() - # back up calibration - calib_name = strftime("mag_calibration_%d_%m_%y_%H_%M_%S",localtime()) - calib_path = os.path.join(self.__location__,calib_name) - os.system("cp %s %s"%(file_path,calib_path)) + # I don't personally need this, but if you want to re-enable it you will need there's a little debugging involved. + # the following error comes up running under ros noetic + # [ERROR] [1697934881.719477]: Error processing request: [Errno 2] No such file or directory: '/data/mag_calibration/last_mag_calibration' ['Traceback (most recent call last):\n', ' File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 633, in _handle_request\n response = convert_return_to_response(self.handler(request), self.response_class)\n', ' File "/home/matthew/catkin_ws/src/calibrate_imu/nodes/calibrate_imu.py", line 143, in calibrate_mag\n f = open(file_path,"w+")\n', "FileNotFoundError: [Errno 2] No such file or directory: '/data/mag_calibration/last_mag_calibration'\n"] + + # file_path = os.path.join(self.__location__,"last_mag_calibration") + # f = open(file_path,"w+") + # pickle.dump(x0,f) + # pickle.dump(L,f) + # pickle.dump(matrix(self.mag_samples),f) + # f.close() + # # back up calibration + # calib_name = strftime("mag_calibration_%d_%m_%y_%H_%M_%S",localtime()) + # calib_path = os.path.join(self.__location__,calib_name) + # os.system("cp %s %s"%(file_path,calib_path)) self.mag_matrix = L self.mag_offset = squeeze(array(x0))