Skip to content
Snippets Groups Projects
Commit ceaa92c0 authored by Bingham, Brian S's avatar Bingham, Brian S
Browse files

ex

parent ce2d8cb7
No related branches found
No related tags found
No related merge requests found
function test_cb(src,msg)
disp('testcb')
end
matlab/genav_init.png

187 KiB

matlab/navsat_init.png

158 KiB

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]
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]')
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
......
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
rosinit;
rc = rosclass2();
rc.init()
% Publish to test
function test_cb(src,msg)
disp('testcb')
end
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment