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

working

parent 13d29e89
No related branches found
No related tags found
No related merge requests found
images/Screenshot from 2017-05-11 14:16:22.png

1.6 MiB

images/local_frame_halligan.png

1.92 MiB

This diff is collapsed.
images/local_frame_halligan_drive.png

1.92 MiB

This diff is collapsed.
classdef PioneerWaypointNode < handle
properties
% ROS communication properties
twist_cmd % Twist command to be published
twist_pub % Twist publisher
msg_twist % Twist command to be published
pub_twist % Twist publisher
sub_odom % Odometry subscription
sub_geonav
sub_navstatus
% Counters
odomN
geonavN
statusN
% Display timer
tmr
% Robot State
x
y
yaw
x_odom;
y_odom;
th_odom;
cnt_odom;
x_geonav;
y_geonav;
th_geonav;
cnt_geonav;
geonav_valid;
% Waypoint control properties
waypoint_list
waypoints
dist_threshold
end
methods
% Constructor method
function obj = PioneerNode()
% This gets called when you instantiate the object.
function init(obj,odom_topic,geonav_topic,...
navstatus_topic,twist_topic,waypoints)
% Initialize robot state
obj.x_odom = nan;
obj.y_odom = nan;
obj.th_odom = nan;
obj.cnt_odom = 0; % counter
obj.x_geonav = nan;
obj.y_geonav = nan;
obj.th_geonav = nan;
obj.cnt_geonav = 0;
obj.geonav_valid = false;
% Setup publishers
twist_pub = publisher(
fprintf('Publishing Twist messages on <%s>\n',twist_topic);
obj.pub_twist = rospublisher(twist_topic,'geometry_msgs/Twist');
obj.msg_twist= rosmessage(obj.pub_twist);
pause(1);
% Setup subscriptions
fprintf('Subscribing to odometry Odometry messages on <%s>\n',odom_topic);
%obj.sub_odom = rossubscriber(odom_topic,'nav_msgs/Odometry',@obj.odom_callback);
obj.sub_odom = rossubscriber(odom_topic,'nav_msgs/Odometry',@test_cb);
fprintf('Subscribing to geonav Odometry messages on <%s>\n',geonav_topic);
%obj.sub_geonav = rossubscriber(geonav_topic,'nav_msgs/Odometry',@obj.geonav_callback);
obj.sub_geonav = rossubscriber('/p5/geonav_odom','nav_msgs/Odometry',@obj.geonav_callback);
%fprintf('Subscribing to nav status messages on <%s>\n',navstatus_topic);
%obj.sub_navstatus = rossubscriber(navstatus_topic,'std_msgs/Int16MultiArray',@obj.navstatus_callback);
% Save the list of waypoints
obj.waypoints = waypoints;
end
% Display status
function display_status(obj)
fprintf('odom[%d]: x=%4.1f y=%4.1f th=%4.1f; geonav[%d]: x=%4.1f, y=%4.1f, th=%4.1f status=%d \n',...
obj.cnt_odom, obj.x_odom, obj.y_odom, obj.th_odom, ...
obj.cnt_geonav, obj.x_geonav, obj.y_geonav, obj.th_geonav);
% Set waypoints explicitly
function set_waypoints(obj,wpts)
obj.waypoints = wpts;
end
% Odometry callback
function odom_callback(obj,src,msg)
disp('odom_callback')
[obj.x_odom,obj.y_odom,obj.th_odom]=obj.odom2xyt(msg);
obj.cnt_odom = obj.cnt_odom + 1;
obj.display_status()
end
% ROS callback methods
function odom_callback(src,msg)
% Geonav callback
function geonav_callback(obj,src,msg)
disp('gcb')
%[obj.x_odom,obj.y_odom,obj.th_odom]=obj.odom2xyt(msg);
end
function geonav_callback(src,msg)
% Nav Status callback
function navstatus_callback(obj,src,msg)
disp('cb')
end
function navstatus_callback(src,msg)
function rfid_callback(obj,src,msg)
% pass
end
function rfid_callback(src,msg)
% Function to convert Odometry to x, y, theta
function [x,y,th] = odom2xyt(obj,omsg)
x = odomMsg.Pose.Pose.Position.X;
y = odomMsg.Pose.Pose.Position.Y;
q = odomMsg.Pose.Pose.Orientation;
angles = quat2eul([q.W,q.X,q.Y,q.Z]);
th = angles(1); % In radians!
end
% Regular methods
function [dist,angvel,linvel] = wcontrol(x,y,th,wp_x,wp_y)
function [dist,angvel,linvel] = wcontrol(obj,x,y,th,wp_x,wp_y)
% Function to implement waypoint guidance
% Inputs:
% - x: Current X position
......@@ -58,29 +127,24 @@ classdef PioneerWaypointNode < handle
% Position Error - distance to waypoint
errorPose=sqrt((x-wp_x)^2+(y-wp_y)^2)
%%% Theta Error
% Theta Error
goaltheta=(atan2((wp_y-y),(wp_x-x)))
errortheta=goaltheta-th
% Clamp angle
while errortheta > pi
errortheta=errortheta-2*pi
end
end
while errortheta<-pi
errortheta=errortheta+2*pi
end
dist=errorPose;
ka = 3.0; % angle gain
angvel= ka*errortheta;
kl = 0.1; % linear gain
linvel=kl*errorPose;
end
end
end
end
dist=errorPose;
angvel= 3*errortheta;
linvel=.1*errorPose;
end
end
end
\ No newline at end of file
% Example of using Pioneer Waypoint Class for single robot waypoint
% guidance
clear
% Try to start ROS - if it is already started, restart
try
rosinit
catch
rosshutdown
rosinit
end
% Setup the parameters to pass to the object
robot='p0';
odom_topic = sprintf('/%s/my_p3at/pose',robot);
geonav_topic = sprintf('/%s/geonav_odom',robot);
navstatus_topic = sprintf('/%s/nav/status',robot);
twist_topic = sprintf('/%s/my_p3at/cmd_vel',robot);
% Instatiate the object
pwn_leader = pioneerwaypointclass();
% Initialize with the parameters - this will start running immediately.
pwn_leader.init(odom_topic,geonav_topic,navstatus_topic,twist_topic,[]);
% Give it some waypoints
pwn_leader.waypoints = [100,30; 90,30; 90 10];
% Enable
pwn_leader.enabled = 1;
% Register the cleanup function so that things exit gracefully
%cleanupObj = onCleanup(@()rosshutdown);
while true
pwn_leader.display_status()
pause(1.0)
end
classdef pioneerwaypointclass < handle
properties
% ROS communication properties
msg_twist % Twist command to be published
pub_twist % Twist publisher
subs cell % cell array of subscribers
% Enabled - does it publish
enabled
% Counters
odomN
geonavN
statusN
% Robot State
x_odom;
y_odom;
th_odom;
cnt_odom;
x_geonav;
y_geonav;
th_geonav;
cnt_geonav;
filter_status;
% Waypoint control properties
waypoints
wpt_index
dist_threshold
% Other
name
end
methods
% Initialization
% This gets called when you instantiate the object.
function init(obj,odom_topic,geonav_topic,...
navstatus_topic,twist_topic,waypoints)
% Default name
obj.name='R0';
% Start disabled
obj.enabled = 0;
% Distance to waypoint threshold
obj.dist_threshold = 1.0;
% Initialize robot state
obj.x_odom = nan;
obj.y_odom = nan;
obj.th_odom = nan;
obj.cnt_odom = 0; % counter
obj.x_geonav = nan;
obj.y_geonav = nan;
obj.th_geonav = nan;
obj.cnt_geonav = 0;
obj.filter_status = nan;
% Setup publishers
fprintf('Publishing Twist messages on <%s>\n',twist_topic);
obj.pub_twist = rospublisher(twist_topic,'geometry_msgs/Twist');
obj.msg_twist= rosmessage(obj.pub_twist);
pause(2);
% Setup subscriptions
obj.subs = {};
fprintf('Subscribing to odometry Odometry messages on <%s>\n',odom_topic);
obj.subs{1} = rossubscriber(odom_topic,'nav_msgs/Odometry',@obj.odom_callback);
fprintf('Subscribing to geonav Odometry messages on <%s>\n',geonav_topic);
obj.subs{2} = rossubscriber(geonav_topic,'nav_msgs/Odometry',@obj.geonav_callback);
fprintf('Subscribing to nav status messages on <%s>\n',navstatus_topic);
obj.subs{3} = rossubscriber(navstatus_topic,'std_msgs/Int16MultiArray',@obj.navstatus_callback);
% Save the list of waypoints
obj.waypoints = waypoints;
obj.wpt_index = 1;
if length(obj.waypoints) > 0
obj.wpt_index = 0;
end
end
% Destructor
function delete(obj)
% delete all the subscriptoins
disp('Deleting subscriptions')
for ii = 1:length(obj.subs)
disp(ii);
%delete(obj.subs{ii});
clear obj.subs;
end
pause(2);
disp('That''s all folks...');
end
% Display status
function display_status(obj)
fprintf('Name=%s: odom[%d]: x=%4.1f y=%4.1f th=%4.1f; geonav[%d]: x=%4.1f, y=%4.1f, th=%4.1f filter_status=%d \n',...
obj.name, obj.cnt_odom, obj.x_odom, obj.y_odom, obj.th_odom, ...
obj.cnt_geonav, obj.x_geonav, obj.y_geonav, obj.th_geonav, obj.filter_status);
if ( (obj.wpt_index > 0) && (length(obj.waypoints) > 0) )
wp_x = obj.waypoints(obj.wpt_index,1);
wp_y = obj.waypoints(obj.wpt_index,2);
else
wp_x = nan;
wp_y = nan;
end
fprintf('Current waypoint[%d of %d]: X=%5.2f, Y=%5.2f; cmd_vel: linear=%4.2f, angular=%4.2f \n',...
obj.wpt_index,length(obj.waypoints),wp_x,wp_y,obj.msg_twist.Linear.X,obj.msg_twist.Angular.Z);
end
% Odometry callback
function odom_callback(obj,src,msg)
obj.cnt_odom = obj.cnt_odom +1;
[obj.x_odom,obj.y_odom,obj.th_odom]=obj.odom2xyt(msg);
%obj.odom2xyt(msg);
%obj.test(msg)
end
% Geonav callback
function geonav_callback(obj,src,msg)
obj.cnt_geonav = obj.cnt_geonav + 1;
[obj.x_geonav,obj.y_geonav,obj.th_geonav]=obj.odom2xyt(msg);
obj.drive2waypoint(obj.x_geonav,obj.y_geonav,obj.th_geonav);
end
% Nav Status callback
function navstatus_callback(obj,src,msg)
status = msg.Data;
obj.filter_status = status(1);
end
function rfid_callback(obj,src,msg)
% pass
end
% Function to convert Odometry to x, y, theta
function [x,y,th] = odom2xyt(obj,msg)
x = msg.Pose.Pose.Position.X;
y = msg.Pose.Pose.Position.Y;
q = msg.Pose.Pose.Orientation;
angles = quat2eul([q.W,q.X,q.Y,q.Z]);
th = angles(1); % In radians!
end
% Drive to the current waypoint with the given position estimate
function drive2waypoint(obj,x,y,th)
if ( (obj.wpt_index == 0) || (obj.wpt_index > length(obj.waypoints)) )
disp('done')
obj.publish_cmd_vel(0,0);
else
wp_x = obj.waypoints(obj.wpt_index,1);
wp_y = obj.waypoints(obj.wpt_index,2);
[dist,angvel,linvel] = obj.wcontrol(x,y,th,wp_x,wp_y);
%disp(dist);
if (dist < obj.dist_threshold)
obj.wpt_index = min(obj.wpt_index+1,length(obj.waypoints));
obj.publish_cmd_vel(0,0);
else
obj.publish_cmd_vel(linvel,angvel);
end
end
end
function publish_cmd_vel(obj,lvel,avel)
obj.msg_twist.Linear.X = lvel;
obj.msg_twist.Angular.Z = avel;
if (obj.enabled)
send(obj.pub_twist,obj.msg_twist);
end
end
% Regular methods
function [dist,angvel,linvel] = wcontrol(obj,x,y,th,wp_x,wp_y)
% Function to implement waypoint guidance
% Inputs:
% - x: Current X position
% - y: Current Y position
% - th: Current theta position
% - wp_x: Current waypoint goals, X
% - wp_y: Current waypoint goals, X
% Outputs:
% - dist: Distance from current position to current waypoint goal
% - angvel: Desired angular velocity [rad/s, clockwise]
% - linvel: Desired linear velocity
% Position Error - distance to waypoint
errorPose=sqrt((x-wp_x)^2+(y-wp_y)^2);
% Theta Error
goaltheta=(atan2((wp_y-y),(wp_x-x)));
errortheta=goaltheta-th;
% Clamp angle
while errortheta > pi
errortheta=errortheta-2*pi;
end
while errortheta<-pi;
errortheta=errortheta+2*pi;
end
dist=errorPose;
ka = 3.0; % angle gain
angvel= ka*errortheta;
kl = 0.1; % linear gain
vel=kl*errorPose;
linvel = sign(vel)*min(abs(vel),1.0);
end
end
end
classdef rosclass < handle
properties
ch_sub
cnt
end
methods
function init(obj)
obj.ch_sub = rossubscriber('/chatter','std_msgs/String',@obj.ch_callback);
obj.cnt=0;
end
function ch_callback(obj,src,msg)
disp(msg.Data);
obj.cnt = obj.cnt + 1;
disp(obj.cnt);
end
end
end
rosinit;
rc = rosclass();
rc.init()
% Publish to test
chatterpub = rospublisher('/chatter', 'std_msgs/String')
pause(2) % Wait to ensure publisher is registered
chattermsg = rosmessage(chatterpub);
for ii=1:100
chattermsg.Data = sprintf('hello world: %d',ii);
send(chatterpub,chattermsg);
pause(0.1);
end
delete(rc);
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