Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update mpu6050_driver.launch to accept MPU YAML file as launch file argument #4

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 26 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ set(CATKIN_DEPS
find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPS})
find_package(Eigen3 REQUIRED)
find_package(cmake_modules REQUIRED)
find_package(yaml-cpp REQUIRED)

catkin_package(
INCLUDE_DIRS include
Expand Down Expand Up @@ -52,6 +53,31 @@ target_link_libraries(mpu6050_calibration_node
${MPU6050_NODE_LIB}
)

add_executable(mpu6050_calibration_node2
src/mpu6050_calibration_node2.cpp
)

target_link_libraries(mpu6050_calibration_node2
${catkin_LIBRARIES}
${MPU6050_NODE_LIB}
yaml-cpp
)

add_executable(mpu6050_node2
src/mpu6050_node2.cpp
)

target_link_libraries(mpu6050_node2
${catkin_LIBRARIES}
${MPU6050_NODE_LIB}
)


# Install Python scripts
catkin_install_python(PROGRAMS scripts/imu_calibration_listener.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
Expand Down
44 changes: 22 additions & 22 deletions config/mpu_settings.yaml
Original file line number Diff line number Diff line change
@@ -1,23 +1,23 @@
# I2C Bus URI used to comunicate with I2C devices (default: "/dev/i2c-1")
bus_uri: "/dev/i2c-1"

# I2C address of MPU6050 (default: 0x68)
bus_uri: /dev/i2c-1
mpu_address: 0x68

# Frequency in Hertz wich IMU data is published (default: 30)
pub_rate: 25

# Frame if of IMU message (default: "imu")
frame_id: "imu"

# Offsets to fix wrong values caused by misalignment
# Sequence is (ax, ay, az, gx, gy, gz) (default: [0, 0, 0, 0, 0, 0])
axes_offsets: [-1987, 44, 1210, 75, -10, 7]

# PID constants used in calibration procedure
ki: 0.2 # (default: 0.1)
kp: 0.1 # (default: 0.1)

# The calibration process is finished when the error is aproximate zero with
# the precision set by delta (default: 0.5)
delta: 0.5
pub_rate: 200
frame_id: imu
axes_offsets: [0, 0, 0, 0, 0, 0]
ki: 0.20000000000000001
kp: 0.10000000000000001
delta: 0.20000000000000001
Xgain: 0.99591491699218748
YtoX: -0.0031604003906250001
ZtoX: 0.081433105468750003
Xofs: -0.00078877766927083336
XtoY: -0.0031121826171875001
Ygain: 0.99959594726562506
ZtoY: 0.0064855957031250003
Yofs: -0.0081247965494791671
XtoZ: -0.098767089843750003
YtoZ: -0.032595825195312497
Zgain: 1.0119696044921875
Zofs: -0.00021769205729166668
gyro_x_offset: -2.460000010728836
gyro_y_offset: 8.7046183967590327
gyro_z_offset: 1.5921755743026733
11 changes: 11 additions & 0 deletions launch/imu_calibration_listener.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<arg name="yaml_file" default="$(find mpu6050_driver)/config/mpu_settings.yaml"/>
<arg name="node_required" defualt="false"/>
<!-- IMU Calibration Listener Node -->
<node name="imu_calibration_listener" pkg="mpu6050_driver" type="imu_calibration_listener.py" output="screen" required="$(arg node_required)">
<!-- Path to the MPU Settings YAML file -->
<!-- Adjust the default path as necessary -->
<param name="mpu_settings_path" value="$(arg yaml_file)"/>
</node>
</launch>
11 changes: 11 additions & 0 deletions launch/mpu6050_calibration2.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<arg name="mpu_yaml" default="$(find mpu6050_driver)/config/mpu_settings.yaml"/>
<arg name="yaml_output_path" default="/home/vio/config/new_mpu_settings.yaml"/>
<!-- Run the calibration node -->
<node name="mpu6050_calibration_node2" pkg="mpu6050_driver" type="mpu6050_calibration_node2" output="screen" required="true">
<rosparam file="$(arg mpu_yaml)" command="load"/>
<param name="num_measurements" value="200" />
<param name="yaml_output_path" value="$(arg yaml_output_path)" />
</node>
</launch>
5 changes: 3 additions & 2 deletions launch/mpu6050_driver.launch
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
<?xml version="1.0"?>
<launch>

<node name="mpu_node" pkg="mpu6050_driver" type="mpu6050_node" output="screen">
<rosparam file="$(find mpu6050_driver)/config/mpu_settings.yaml" command="load"/>
<arg name="mpu_yaml" default="$(find mpu6050_driver)/config/mpu_settings.yaml"/>
<node name="mpu_node" pkg="mpu6050_driver" type="mpu6050_node" output="screen">
<rosparam file="$(arg mpu_yaml)" command="load"/>
</node>

</launch>
8 changes: 8 additions & 0 deletions launch/mpu6050_driver2.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
<?xml version="1.0"?>
<launch>
<arg name="mpu_yaml" default="$(find mpu6050_driver)/config/mpu_settings.yaml"/>

<node name="mpu_corrected_node" pkg="mpu6050_driver" type="mpu6050_node2" output="screen">
<rosparam file="$(arg mpu_yaml)" command="load"/>
</node>
</launch>
77 changes: 77 additions & 0 deletions scripts/imu_calibration_listener.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
import yaml
import os
import time

class FlowStyleListDumper(yaml.Dumper):
def increase_indent(self, flow=False, indentless=False):
return super(FlowStyleListDumper, self).increase_indent(flow=True)

class IMUCalibrationListener:
def __init__(self):
self.node_name = 'mpu_calibration_listener'
rospy.init_node(self.node_name, anonymous=True)

self.yaml_path = rospy.get_param('~mpu_settings_path', '')
if not self.yaml_path:
rospy.logerr("YAML file path must be specified as a private parameter.")
exit(1)

self.last_time_received = time.time()
self.timeout_duration = 5 # Time in seconds to wait after the last message
self.last_offsets = None

rospy.Subscriber('/imu_offsets', Imu, self.imu_callback)

rospy.loginfo(f"{self.node_name} started.")
rospy.loginfo("Please ensure the IMU is placed level on a flat surface and remains motionless during calibration.")
rospy.loginfo(f"Loading configuration from: {self.yaml_path}")

def imu_callback(self, data):
self.last_time_received = time.time()
self.last_offsets = [
data.linear_acceleration.x,
data.linear_acceleration.y,
data.linear_acceleration.z,
data.angular_velocity.x,
data.angular_velocity.y,
data.angular_velocity.z
]

def save_offsets_to_yaml(self):
# Load the original YAML file
with open(self.yaml_path, 'r') as file:
data = yaml.safe_load(file)

# Update the offsets
data['axes_offsets'] = [int(offset) for offset in self.last_offsets]

# Generate the path for the new YAML file
new_yaml_path = os.path.join(os.path.dirname(self.yaml_path), 'new_mpu_settings.yaml')

# Save the updated data in a new YAML file using the custom settings
with open(new_yaml_path, 'w') as file:
yaml.dump(data, file, Dumper=FlowStyleListDumper, default_flow_style=None)

rospy.loginfo(f"Calibration complete. Offsets saved to: {new_yaml_path}")

def run(self):
rate = rospy.Rate(1) # Check every second
while not rospy.is_shutdown():
current_time = time.time()
if current_time - self.last_time_received > self.timeout_duration:
if self.last_offsets:
rospy.loginfo("No new messages from the calibration node. Assuming convergence.")
self.save_offsets_to_yaml()
rospy.signal_shutdown("Calibration Converged")
else:
rospy.logwarn("Timeout reached but no offsets received.")
else:
rospy.loginfo("Waiting for measurements to converge...")
rate.sleep()

if __name__ == '__main__':
imu_cal_listener = IMUCalibrationListener()
imu_cal_listener.run()
6 changes: 3 additions & 3 deletions src/mpu6050.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,9 @@ void MPU6050::initialize(const std::string& i2c_bus_uri) {

setClockSource(MPU6050_CLOCK_PLL_XGYRO);

setFullScaleGyroRange(MPU6050_GYRO_FS_250);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
setDLPFMode(MPU6050_DLPF_BW_10);
setFullScaleGyroRange(MPU6050_GYRO_FS_1000); // it was MPU6050_GYRO_FS_250
setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // it was MPU6050_ACCEL_FS_2
setDLPFMode(MPU6050_DLPF_BW_42); // it was MPU6050_DLPF_BW_10

accel_lsb_sensitivity_ = this->getAccelLSBSensitivity();
gyro_lsb_sensitivity_ = this->getGyroLSBSensitivity();
Expand Down
2 changes: 1 addition & 1 deletion src/mpu6050_calibration_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ void MPU6050CalibrationNode::computeOffsets() {
float dt = 1.0 / pub_rate_; // How it isn't a dynamic system, sample time doesn't must exactly computed

IMUData<int16_t> imu_raw_data = mpu6050_.getRawMotion6();
imu_raw_data.accel.z -= 16384; // Remove gravity contribution
imu_raw_data.accel.z -= 8192; // Remove gravity contribution 8192: +-4g, 16384: +-2g, 2048: +-8g, 2048: 16+-g

/* The divisions here is beacause the offsets need to be set when the MPU is
in the less sensitive mode (accel in 16g mode and gyro in 2000 degrees/sec mode).
Expand Down
Loading