Homework 9: Its coming together (part 2)
MECH 5970/6970 Software for Sensors & Systems
Due: Tuesday 4-9-20 at 11:59 PM
Your assignment is to extend your previous assignment to also incorporate magnetometer information. Hopefully you will see the usefulness of classes.
Your program will:
• Subscribe to the imu and magnetometer readings from the sensor driver you’ve made (hw#7)
• Run these measurements through the filter that you’ve previously made (hw#5)
• Publish another topic with the sensor_msgs::Imu data type containing the accelerometer and gyroscope measurements from the imu topic that you subscribed (e.g. it will be repeating those measurements) However, this time the quaternion data field will be populated based on your filter output.
Steps:
• Modify your HW7 source code such that your IMU and magnetic field messages are referencing the same timestamp. Recommit your source code (1 pt).
• Make and checkout the new branch of your source code from HW#8. Call this one hw9-devel.
• Modify your subscriber for imu information and create a second subscriber for magnetometer information using message filters (http://wiki.ros.org/message_filters) (1 pt)
• Create and register a callback that takes in inputs from both IMU and Magnetic field messages. Use this callback instead if the IMU callback from HW#8 and make another instance of Imu_filt within the global namespace, Call this one magFilt. Initialize with k=0.8 for now. (hint: http://wiki.ros.org/message_filters) (1 pt)
• Within your callback, run the accFilt.update accelerometer and gyroscope measurements, use 0.0078 for dT. Similarly, run magFilt.update, but with magnetometer measurements instead of accelerometer measurements. Use 0.0078 for dT. (1 pt)
• Transcribe the attached matlab code into C++ in order to calculate a quaternion. Use the values returned by accFilt.getOutput and magFilt.getOutput for the accelerometer and magnetometer values. (Hint: you can build up from the incAccel function from HW#7) (1 pt)
• Publish the quaternion using sensor_msgs::Imu using the values you subscribed (step #4) but populate orientation.w, orientation.x, orientation.y, and orientation.z with the 1st, 2nd, 3rd, and 4th element of your quaternion (1 pt)
• Specify your k values for the mag and accel using rosparam, default to 0.8. Specify sampling period using rosparam as well (1 pt)
• Write a launch file for your node (1 pt)
• Push and commit your code to the appropriate branch (1 pt)
Hints: Look at other similar repositories
https://github.com/KumarRobotics/kr_attitude_eskf
https://github.com/ccny-ros-pkg/imu_tools
Bonus ( 3 pts) If things are working, commit your code and switch to a new branch and encase all your functions and instance declarations into a class (2 pt) (Hint: you will need to use initialization lists and the this pointer).
Your main function should look like the following:
main(int argc, char **argv)
{
ros::init(argc, argv, “imu_comp”);
ImuNode imu;
ros::spin();
return 0;
}
Bonus 2 pts (replace sampling period based on what is calculated from the time stamps).
function [ q ] = accelMag( accel,mag)
%ACCELMAG calculates orientation using accelerometer & magnetometers
% calculations is done in yaw-pitch-roll and converted to quaternions
% accel: nx3 vector with accelerometer measurements (gravity vector)
% mag: nx3 vector with magnetometer measurements (magetic north vector)
% q: nx4 quaternion rotation vector (q1 is real)
%
% Author: Howard Chen
% Date Created: 3/2/2017
% calculate pitch and roll from accel
%pitch
pitch = atan2(-accel(:,1),sqrt(accel(:,2).^2+accel(:,3).^2));
%roll
roll = atan2(accel(:,2),accel(:,3));
% calculate yaw from mag and accel
yaw=atan2(mag(:,3).*sin(roll)-mag(:,2).*cos(roll),mag(:,1).*cos(pitch)+mag(:,2).*sin(pitch).*sin(roll)+mag(:,3).*sin(pitch).*cos(roll));
% convert ypr to quaternion
q(:,1) = cos(yaw./2).*cos(pitch./2).*cos(roll./2)+sin(yaw./2).*sin(pitch./2).*sin(roll./2);
q(:,2) = cos(yaw./2).*cos(pitch./2).*sin(roll./2)-sin(yaw./2).*sin(pitch./2).*cos(roll./2);
q(:,3) = cos(yaw./2).*sin(pitch./2).*cos(roll./2)+sin(yaw./2).*cos(pitch./2).*sin(roll./2);
q(:,4) = sin(yaw./2).*cos(pitch./2).*cos(roll./2)-cos(yaw./2).*sin(pitch./2).*sin(roll./2);
end