diff --git a/matlab/display_status.m b/matlab/display_status.m new file mode 100644 index 0000000000000000000000000000000000000000..20b2d6a40d59ff4780af40a3899d8aedd1cffae6 --- /dev/null +++ b/matlab/display_status.m @@ -0,0 +1,3 @@ +function test_cb(src,msg) +disp('testcb') +end diff --git a/matlab/genav_init.png b/matlab/genav_init.png new file mode 100644 index 0000000000000000000000000000000000000000..e0e72afa92956ac9366fbe1e7cee80d3d3034e67 Binary files /dev/null and b/matlab/genav_init.png differ diff --git a/matlab/navsat_init.png b/matlab/navsat_init.png new file mode 100644 index 0000000000000000000000000000000000000000..c305921beddb28ba79137dd13eae91d869698fbf Binary files /dev/null and b/matlab/navsat_init.png differ diff --git a/matlab/pioneer_los.m b/matlab/pioneer_los.m new file mode 100644 index 0000000000000000000000000000000000000000..da19443899831b427886356a02c2441451fff5fc --- /dev/null +++ b/matlab/pioneer_los.m @@ -0,0 +1,142 @@ +function [linvel,angvel] = pioneer_los(x,y,th,xstart,ystart,xend,yend) + +% pioneer_los controller +% Line of sight controller for pioneer +% Inputs: +% x: current x position of robot +% y: +% th: +% xstart,ystart: starting point of the line +% xend,yend: ending point (goal) of the line + + +% Bearing from current location to end point +angle2end = (atan2((yend-y),(xend-x))*180/pi); %*(-1) + 90 + + +anglelinerad = atan2(yend-ystart,xend-xstart); + +% Slope of line +if ((xend-xstart)==0) + m = 10000; +else + m = (yend-ystart)/(xend-xstart); + +% Intersection between robot current location and line +if (m==0) + yint = ystart; + xint = x; +elseif (abs(m) > 1000) + yint = y; + xint = xstart +else + xint = (m/(m^2+1))*(y-ystart+m*xstart+x/m) + yint = ystart+m*(xint-xstart) +end + +% Distance to line +d2line = sqrt((x-xint)**2+(y-yint)**2); +% msg = "LOS:xint: %5.2f,yint: %5.2f,m: %5.2f"%(xint,yint,m) +angle2line = atan2(yint-y,xint-x)*180/pi; +% Calculate the point to aim at +dist2goal = distance2end(behavior,state); +if (dist2goal > Ggain['lineDelta']) + xtgt = xint+Ggain['lineDelta']*cos(anglelinerad) + ytgt = yint+Ggain['lineDelta']*sin(anglelinerad) +else + xtgt = xend + ytgt = yend +end + +%print "%f:%f"%(state[0],state[1]) +%bearing2line = -angle2line + 90.0 +if (m == 0) + if (yint > y) + GlineInt = GlineInt + d2line*GdT + else + GlineInt = GlineInt - d2line*GdT + end +elseif (abs(m) > 1000) + if (xint > x) + GlineInt = GlineInt + d2line*GdT + else + GlineInt = GlineInt - d2line*GdT + end +else + if (angle2line > 0 and angle2line < 180) + GlineInt = GlineInt + d2line*GdT + else + GlineInt = GlineInt - d2line*GdT + +GlineInt = saturate(GlineInt, Ggain['lineWindup']) +if GoldD2line != d2line: + GlineDev = (d2line - GoldD2line)/GdT +end + +% set a hard max. on the derivative - this is a hack! +if abs(GlineDev) > 10.0: + GlineDev = 0.0 +GlineDev = saturate(GlineDev,2.0) + +%if angle2end < 0: +% angle2end = 360 + angle2end +%if angle2line < 0: +% angle2line = 360 + angle2line + +if m ==0: + if xend > xstart: + GlineDev = -GlineDev + end +elseif abs(m) > 1000: + if yend < ystart: + GlineDev = -GlineDev + end +else + if xend < xstart: + GlineDev = -GlineDev + end +end + +% Calculate angle to the goal +angleP = atan2(ytgt-state[1],xtgt-state[0])*180/pi +if angleP < 0: + angleP = 360+angleP +end +angleD = -Ggain['losKd']*GlineDev +if angle2end >= angle2line: + angleGoal = angleP+angleD +else + angleGoal = angleP-angleD +end + +hdgGoal = -1.0*angleGoal + 90.0 +if hdgGoal < -180: + hdgGoal = hdgGoal +360 +end +if hdgGoal > 180: + hdgGoal = hdgGoal - 360 +end +behavior.behavior_t.heading = hdgGoal + +ThrustHdg = HeadingControl(behavior,state) +% The speed can be set a few ways +if (behavior.behavior_t.velocity != -1) and (behavior.behavior_t.velocity != None): + % slow down when we get close + if abs(dist2goal) < 5.0: + behavior.behavior_t.velocity = min(behavior.behavior_t.velocity,abs(dist2goal)/5.0) + end + ThrustVel = VelocityControl(behavior,state) + %print "vel control" +else: + ThrustVel = DistanceControl2(behavior,state) +end + +Tport = saturate(ThrustHdg[0]+ThrustVel[0],1.0) +Tstbd = saturate(ThrustHdg[1]+ThrustVel[1],1.0) + +msg ="LOS: dist2goal: %5.1f, dist2line: %5.2f, oldd2line: %5.2f, angle2line %5.1f, hdgGoal=%5.2f, angleGoal=%5.2f, angleP: %5.2f, angleD: %5.3f, GlineDev: %4.7f, Tp=%5.2f, Ts=%5.2f"% \ +(dist2goal,d2line, GoldD2line,angle2line,hdgGoal,angleGoal,angleP,angleD,GlineDev,Tport,Tstbd) +if GoldD2line != d2line: + GoldD2line = d2line +end +return [Tport,Tstbd,behavior,msg] diff --git a/matlab/post_nav_status.m b/matlab/post_nav_status.m new file mode 100644 index 0000000000000000000000000000000000000000..0a7a10c80f9a5b82a9c07fca617ea705d0023691 --- /dev/null +++ b/matlab/post_nav_status.m @@ -0,0 +1,192 @@ +close('all') + +% Create a bag file object with the file name +%bag = rosbag('../data/perimeter_all3.bag') +bag = rosbag('2017-05-30-20-02-36.bag'); + +% Display a list of the topics and message types in the bag file +bag.AvailableTopics + + +%% Odometry estimate from P3AT +ns='p1'; +bagselect = select(bag,'Topic',['/' ns '/my_p3at/pose']); +% Create a time series object based on the fields of the message type +ts_odom = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',... + 'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',... + 'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',... + 'Twist.Twist.Linear.X'); + +% Extract the data as simple vectors. +x_odom = ts_odom.Data(:,1); +y_odom = ts_odom.Data(:,2); +angles = quat2eul([ts_odom.Data(:,6), ts_odom.Data(:,3),ts_odom.Data(:,4),ts_odom.Data(:,5)]); +yaw_odom = angles(:,1); +vel_odom=ts_odom.Data(:,7); + +figure(); +clf(); +plot(x_odom,y_odom); +hold on +plot(x_odom(1),y_odom(1),'go','markerfacecolor','g') +plot(x_odom(end),y_odom(end),'ro','markerfacecolor','r') +xlabel('Odom X [m]') +ylabel('Odom Y [m]') +title('Raw Odometry Estimate') +title('Odometry Estimate') +legend('Path','Start','End') +axis('equal') +grid on; + +%% Geonav from GPS/IMU +bagselect = select(bag,'Topic',['/' ns '/geonav_odom']); + +% Create a time series object based on the fields of the message type +ts_nav = timeseries(bagselect,'Pose.Pose.Position.X','Pose.Pose.Position.Y',... + 'Pose.Pose.Orientation.X','Pose.Pose.Orientation.Y',... + 'Pose.Pose.Orientation.Z','Pose.Pose.Orientation.W',... + 'Twist.Twist.Linear.X'); +% Extract the data as simple vectors. +x = ts_nav.Data(:,1); +y = ts_nav.Data(:,2); +angles = quat2eul([ts_nav.Data(:,6), ts_nav.Data(:,3),ts_nav.Data(:,4),ts_nav.Data(:,5)]); +yaw = angles(:,1); +vel=ts_nav.Data(:,7); + +% Plot the path with endpoints +figure(); +clf(); +plot(x,y); +hold on +plot(x(1),y(1),'go','markerfacecolor','g') +plot(x(end),y(end),'ro','markerfacecolor','r') +xlabel('Geonav X [m]') +ylabel('Geonav Y [m]') +legend('Path','Start','End') +axis('equal') +grid on; + +% Quiver plot to check the heading alignment. +figure(); +clf() +u = vel.*cos(yaw); +v = vel.*sin(yaw); +ii = 1:10:length(x); % Decimate the data so that it plot only every Nth point. +quiver(x(ii),y(ii),u(ii)*10,v(ii)*10); +hold on +plot(x(1),y(1),'go','markerfacecolor','g') +plot(x(end),y(end),'ro','markerfacecolor','r') +xlabel('Geonav X [m]') +ylabel('Geonav Y [m]') +axis('equal') +grid on; + +% Extract the covariance estimates from the navigation solution +% as a measure of navigation quality +varX = []; +varY = []; +varYaw = []; +msgs = readMessages(bagselect); +for ii = 1:length(msgs) + varX(ii) = msgs{ii}.Pose.Covariance(1); + varY(ii) = msgs{ii}.Pose.Covariance(8); + varYaw(ii) = msgs{ii}.Pose.Covariance(22); +end + +figure(4); +clf(); +subplot(3,1,1) +t0 = ts_nav.Time-ts_nav.Time(1); +semilogy(t0/60,sqrt(varX),t0/60,sqrt(varY)); +legend('\sigma_x','\sigma_y'); +ylabel('Geonav Pose StdDev [m]') +grid('on') +ylim([0.1,100]) +subplot(3,1,2) +semilogy(t0/60,sqrt(varYaw)*180/pi); +grid('on') +ylabel('Geonav Yaw StdDev [deg]') + +%% GPS statu +bagselect = select(bag,'Topic',['/' ns '/gps/fix']); +msgs = readMessages(bagselect); +ts_gps = timeseries(bagselect); +gpsVarX = []; +gpsVarY = []; +gpsVarZ = []; +gpsStatus = []' +for ii = 1:length(msgs) + gpsVarX(ii) = msgs{ii}.PositionCovariance(1); + gpsVarY(ii) = msgs{ii}.PositionCovariance(5); + gpsVarZ(ii) = msgs{ii}.PositionCovariance(9); + gpsStatus(ii) = msgs{ii}.Status.Status; +end + + +figure(5); +clf(); +subplot(2,1,1) +t3 = ts_gps.Time-ts_nav.Time(1); +semilogy(t3/60,gpsVarX,t3/60,gpsVarY); +legend('\sigma_x^2','\sigma_y^2'); +ylabel('GPS Position Variance [m^2]') +grid('on') +ylim([0.1 10000]) +subplot(2,1,2) +plot(t3/60,gpsStatus) +xlabel('Time [s]') +ylabel('GPS Fix Status') +grid('on') + + +%% Navigation Sensor Status +% Plot the status of the navigation solution +bagstatus = select(bag,'Topic',['/' ns '/nav/status']); +msgs = readMessages(bagstatus); +status = [] +for ii = 1:length(msgs) + status(ii,:) = msgs{ii}.Data; +end +ts_status = timeseries(bagstatus); + +figure(); +clf(); +t1 = ts_status.Time - ts_status.Time(1); +plot(ts_status.Time,status) +legend('FilterState','DynamicsMode','StatusFlags'); +title('Navigation Kalman Filter Status') +xlabel('Time [s]') +ylabel('Value') + +figure(4); +subplot(3,1,3) +plot(t1/60,status(:,1)) +xlabel('Time [min]') +ylabel('Microstain Filter Status') +grid('on') + + + +%% Compare odometry and geonav estimates +% Normalize the odometry with the initial geonav estimate +dyaw = yaw_odom(1)-yaw(1); % rotation angle for odom frame +dx = x_odom-x_odom(1); +dy = y_odom-y_odom(1); +x_o = (dx.*cos(dyaw)-dy.*sin(dyaw))+x(1); +y_o = (dx.*sin(dyaw)+dy.*cos(dyaw))+y(1); + +figure(); +clf(); +plot(x_o,y_o,x,y); +legend('Odometry','Geonav'); +hold on +plot(x(1),y(1),'go','markerfacecolor','g') +plot(x(end),y(end),'ro','markerfacecolor','r') +plot(x_o(1),y_o(1),'go','markerfacecolor','g') +plot(x_o(end),y_o(end),'ro','markerfacecolor','r') +grid on; +axis('equal') +xlabel('X [m]') +ylabel('Y [m]') + + diff --git a/matlab/post_odom.m b/matlab/post_odom.m index 89656a26c6812992189ba8469aff3564841bc3c2..ebd38f93624de41f9ca9973b6b846f3bfe79db16 100644 --- a/matlab/post_odom.m +++ b/matlab/post_odom.m @@ -1,7 +1,8 @@ close('all') % Create a bag file object with the file name -bag = rosbag('../data/perimeter_all3.bag') +%bag = rosbag('../data/perimeter_all3.bag') +bag = rosbag('2017-05-30-20-02-36.bag'); % Display a list of the topics and message types in the bag file bag.AvailableTopics diff --git a/matlab/rosclass2.m b/matlab/rosclass2.m new file mode 100644 index 0000000000000000000000000000000000000000..c7e3b5f0e7d84c662d017ad0736f2dda42e5595e --- /dev/null +++ b/matlab/rosclass2.m @@ -0,0 +1,18 @@ +classdef rosclass2 < handle + + properties + ch_sub + end + + methods + function init(obj) + obj.ch_sub = rossubscriber('/p5/geonav_odom','nav_msgs/Odometry',@obj.ch_callback); + end + function ch_callback(obj,src,msg) + disp('ch'); + end + + end + +end + diff --git a/matlab/rosclass_ex2.m b/matlab/rosclass_ex2.m new file mode 100644 index 0000000000000000000000000000000000000000..ee25f5c3b3fe3070cbd74debae91e7c3edbf5048 --- /dev/null +++ b/matlab/rosclass_ex2.m @@ -0,0 +1,9 @@ + +rosinit; + +rc = rosclass2(); +rc.init() + +% Publish to test + + diff --git a/matlab/test_cb.m b/matlab/test_cb.m new file mode 100644 index 0000000000000000000000000000000000000000..20b2d6a40d59ff4780af40a3899d8aedd1cffae6 --- /dev/null +++ b/matlab/test_cb.m @@ -0,0 +1,3 @@ +function test_cb(src,msg) +disp('testcb') +end