Homework 8: Its coming together
MECH 5970/6970 Software for Sensors & Systems
Due: Thursday 4-2-20 at 11:59 PM
Your assignment is to write a ROS wrapper for the filter that you have made. If your HW#5 is not working, please send me an email and I will provide you with the solution. This folder also contains the matlab implementation of this code.
Your program will:
• Subscribe to the imu 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:
• Create a new catkin package called imu-comp with the following dependencies: roscpp, sensor_msgs, message_filters and commit the package to Git on the branch hw8-devel (the folder catkin_ws/src/imu_comp) (1 pt)
• Create a subscriber for imu information (1 pt)
• Create and register a callback that takes in inputs from IMU data (1 pt)
• Copy and paste your header file from HW#5 into the ‘include’ folder of your package. Copy and paste your implementation file from HW#5 into the ‘src’ file of your package. (1 pt)
• Make an instance of Imu_filt within the global namespace called accFilt. Initialize with k=0.8 for now. Be sure to modify your header file location and CMakeLists so that your filter compiles (1 pt)
• Within your callback, run the accFilt.update accelerometer and gyroscope 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 (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)
• Push and commit your code (1 pt)
Hints: Look at other similar repositories
https://github.com/KumarRobotics/kr_attitude_eskf
https://github.com/ccny-ros-pkg/imu_tools
function [ q ] = incAccel( accel)
%pitch
pitch = atan2(-accel(:,1),sqrt(accel(:,2).^2+accel(:,3).^2));
%roll
roll = atan2(accel(:,2),accel(:,3));
% convert to quaternion
q(:,1) = cos(pitch./2).*cos(roll./2)+sin(yaw./2).*sin(pitch./2).*sin(roll./2);
q(:,2) = cos(pitch./2).*sin(roll./2)-sin(yaw./2).*sin(pitch./2).*cos(roll./2);
q(:,3) = sin(pitch./2).*cos(roll./2)+sin(yaw./2).*cos(pitch./2).*sin(roll./2);
q(:,4) = cos(pitch./2).*cos(roll./2)-cos(yaw./2).*sin(pitch./2).*sin(roll./2);
end