├── MATLAB ├── PG-ELLA │ ├── Library │ │ ├── PGELLAUpdate.m │ │ ├── computeHessianTurtule.m │ │ ├── initPGELLA.m │ │ ├── performActionPGELLA.m │ │ └── updatePGELLA.m │ ├── pg_ella_startup.m │ ├── pg_ellainitpolicyCallback.m │ ├── pg_ellainputCallback.m │ ├── sendNextTask.m │ ├── sendPolicies.m │ └── testPGELLA.m └── PG │ ├── Library │ ├── DlogPiDTheta.m │ └── episodicREINFORCE.m │ ├── launch_pg.m │ ├── pg_startup.m │ ├── pginitpolicyCallback.m │ ├── pginputCallback.m │ └── testPG.m ├── lilee_follower ├── CHANGELOG.rst ├── CMakeLists.txt ├── cfg │ ├── .gitignore │ └── Follower.cfg ├── launch │ ├── follower.launch │ └── includes │ │ ├── safety_controller.launch.xml │ │ └── velocity_smoother.launch.xml ├── package.xml ├── param │ └── mux.yaml ├── plugins │ └── nodelets.xml └── src │ ├── .gitignore │ └── lilee_follower.cpp ├── lilee_gazebo ├── CMakeLists.txt ├── cmake │ ├── lilee_gazeboConfig-version.cmake │ └── lilee_gazeboConfig.cmake ├── launch │ ├── edit_maze.launch │ ├── edit_maze_pg.launch │ ├── edit_maze_qlearning.launch │ ├── includes │ │ ├── create.launch.xml │ │ ├── kobuki.launch.xml │ │ └── roomba.launch.xml │ ├── launch_maze.launch │ ├── launch_maze_pg.launch │ └── launch_maze_qlearning.launch ├── maps │ ├── playground.pgm │ └── playground.yaml ├── package.xml ├── src │ ├── environment.cpp │ └── world_control.cpp ├── terminal_cmds └── worlds │ ├── empty.world │ ├── lilee.world │ ├── maze.world │ ├── maze_backup.world │ ├── maze_pg.world │ ├── maze_qlearning.world │ └── playground.world ├── lilee_rl_continuous ├── CMakeLists.txt ├── include │ └── lilee_rl_continuous │ │ ├── actions.h │ │ ├── gazebo_parameters.h │ │ └── world_control.h ├── launch │ ├── includes │ │ ├── velocity_smoother.launch.xml │ │ └── velocity_smoother.launch.xml~ │ ├── pg-ella.launch │ └── pg.launch ├── msg │ ├── AgentLocation.msg │ ├── ContinuousAction.msg │ ├── GoalLocation.msg │ ├── State.msg │ └── TeleopCmd.msg ├── package.xml ├── param │ └── mux.yaml ├── plugins │ └── nodelets.xml ├── src │ ├── actions.cpp │ ├── agent_pg-ella.cpp │ ├── agent_pg.cpp │ ├── environment.cpp │ ├── teleop.cpp │ └── world_control.cpp └── srv │ ├── AgentEnvironment.srv │ └── WorldControl.srv ├── lilee_rl_discrete ├── CMakeLists.txt ├── include │ └── lilee_rl_discrete │ │ ├── actions.h │ │ └── gazebo_parameters.h ├── launch │ ├── includes │ │ └── velocity_smoother.launch.xml │ └── qlearning.launch ├── msg │ ├── AgentLocation.msg │ ├── GoalLocation.msg │ ├── State.msg │ └── TeleopCmd.msg ├── package.xml ├── param │ └── mux.yaml ├── plugins │ └── nodelets.xml ├── src │ ├── actions.cpp │ ├── agent_qLearning.cpp │ ├── environment.cpp │ └── teleop.cpp └── srv │ └── AgentEnvironment.srv ├── lilee_tracker ├── CMakeLists.txt ├── lilee_tracker.jpg ├── package.xml └── scripts │ ├── thresh.txt │ └── tracker.py └── readme /MATLAB/PG-ELLA/Library/PGELLAUpdate.m: -------------------------------------------------------------------------------- 1 | function [model]=PGELLAUpdate(model,HessianArray,ParameterArray,taskIndex) 2 | % Update S(:,taskIndex) 3 | 4 | 5 | Dsqrt=eye(2,2); 6 | %Dsqrt = HessianArray(taskIndex).D^.5; 7 | %Dsqrt = HessianArray(taskIndex).D; 8 | target = Dsqrt*ParameterArray(taskIndex).alpha; 9 | dictTransformed = Dsqrt*model(1).L; 10 | 11 | s = full(mexLasso(target,dictTransformed,struct('lambda',model(1).mu_one/2))); 12 | 13 | model.S(:,taskIndex)=s; 14 | % Update L using GD for now... Implemented for consecutive tasks for now 15 | numTasks=model(1).T; 16 | %size(HessianArray); 17 | %size(ParameterArray); 18 | %taskIndex; 19 | termOne=zeros(size(model(1).L)); 20 | termTwo=zeros(size(model(1).L)); 21 | termThree=zeros(size(model(1).L)); 22 | 23 | for i=1:numTasks 24 | %HessianArray(i).D 25 | %ParameterArray(i).alpha 26 | %pause 27 | 28 | termOne=termOne+HessianArray(i).D*ParameterArray(i).alpha*model(1).S(:,i)'; 29 | termTwo=termTwo+HessianArray(i).D*model(1).L*model(1).S(:,i)*model(1).S(:,i)'; 30 | termThree=termThree+2*model(1).mu_one/2*model(1).L; 31 | end 32 | 33 | der=1./numTasks*(-2*termOne+2*termTwo+termThree); 34 | 35 | disp('der') 36 | disp(der) 37 | disp('model(1).L') 38 | disp(model(1).L) 39 | 40 | model(1).L=model(1).L+model(1).learningrate*der; 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/Library/computeHessianTurtule.m: -------------------------------------------------------------------------------- 1 | function [D]=computeHessianTurtule(data,policy) 2 | 3 | sigma=policy.theta.sigma; 4 | numRollouts=size(data,2); 5 | Hes=zeros(2,2); 6 | for i=1:numRollouts 7 | VarOne=data(i).x(1,:); 8 | Reward=data(i).r; 9 | VarOneSquared=sum(VarOne.^2); 10 | VarTwo=data(i).x(2,:); 11 | VarTwoSquared=sum(VarTwo.^2); 12 | %VarThree=data(i).x(3,:); 13 | %VarThreeSquared=sum(VarThree.^2); 14 | %VarFour=data(i).x(4,:); 15 | %VarFourSquared=sum(VarFour.^2); 16 | RewardDum=sum(Reward); 17 | Matrix=1./sigma*([VarOneSquared 0; 0 VarTwoSquared]*RewardDum); 18 | %Matrix=([PosSquare PosVel; PosVel VelSquare]*RewardDum); 19 | Hes=Hes+Matrix; 20 | end 21 | 22 | D=-Hes*1./numRollouts; 23 | -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/Library/initPGELLA.m: -------------------------------------------------------------------------------- 1 | function [model]=initPGELLA(dimTasks,dimLatent,pg_Rate,reg_Rate,numTasks) 2 | % Documentation stuff 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % ELLA-Params 5 | model.L=zeros(dimTasks,dimLatent); 6 | model.A=zeros(dimLatent*dimTasks,dimLatent*dimTasks); 7 | model.b=zeros(dimLatent*dimTasks,1); 8 | model.T=0; 9 | model.mu=reg_Rate; 10 | model.S=zeros(dimLatent,numTasks); 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | % PG params 13 | % PGparam.learning -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/Library/performActionPGELLA.m: -------------------------------------------------------------------------------- 1 | function [policy]=performActionPGELLA(model,taskIndex) 2 | 3 | thetaTask=model(1).L*model(1).S(:,taskIndex); 4 | policy.theta.k=thetaTask; 5 | policy.theta.sigma=0.1;%rand(); 6 | policy.type=3; -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/Library/updatePGELLA.m: -------------------------------------------------------------------------------- 1 | function [model,HessianArray,ParameterArray]=updatePGELLA(data,model,taskIndex,pg_Param,policies,HessianArray,ParameterArray) 2 | 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | global numTasks; 5 | 6 | 7 | %if (model(1).T > numTasks) 8 | % return; 9 | %end 10 | 11 | % Still model.T .. needs update --> Check if you have a new task ...,HessianArray,ParameterArray 12 | % Perform policy update step 13 | if (model(1).T < numTasks-1) 14 | model(1).T=model(1).T+1; % Every time this guy is called there is a new task ... ,HessianArray,ParameterArray 15 | else 16 | return; 17 | end 18 | learningRate=pg_Param(taskIndex).learningrate; 19 | 20 | dJdTheta=episodicREINFORCE(policies(taskIndex).policy,data,pg_Param(taskIndex).param); 21 | % Update Policy Parameters 22 | policies(taskIndex).policy.theta.k=policies(taskIndex).policy.theta.k ... 23 | +learningRate*dJdTheta(1:pg_Param(taskIndex).param.N,1); 24 | % Commented out for now .. 25 | policies(taskIndex).policy.theta.sigma=policies(taskIndex).policy.theta.sigma ... 26 | +policies(taskIndex).policy.theta.sigma.^2*learningRate*dJdTheta(pg_Param(taskIndex).param.N+1,1); 27 | %policies(taskIndex).policy.theta.k 28 | %pause 29 | 30 | % Compute the Hessian 31 | D=computeHessianTurtule(data,policies(taskIndex).policy); % we might need to sample again 32 | %D=eye(2,2); % TODO: Change this back 33 | 34 | HessianArray(taskIndex).D=D; 35 | ParameterArray(taskIndex).alpha=policies(taskIndex).policy.theta.k; 36 | 37 | %HessianArray(taskIndex).D 38 | %ParameterArray(taskIndex).alpha 39 | %pause 40 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 41 | % PG-ELLA Updates 42 | % Update L & S(:taskIndex) 43 | [model]=PGELLAUpdate(model,HessianArray,ParameterArray,taskIndex); 44 | 45 | -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/pg_ella_startup.m: -------------------------------------------------------------------------------- 1 | global LinearVelPolicyMsg; 2 | global AngularVelPolicyMsg; 3 | global States1; 4 | global States2; 5 | global Actions1; 6 | global Actions2; 7 | global Rewards; 8 | global numLearningRates; 9 | 10 | global numEpisodes; 11 | global numEpisodesTilUpdate; 12 | 13 | global numTrajectories; 14 | global numTrajectoriesTilUpdate; 15 | 16 | global taskIndex_; 17 | global numTasks; 18 | global dimStateSpace; % dimension of state space 19 | 20 | global model1; 21 | global model2; 22 | global pg_Param1; 23 | global pg_Param2; 24 | global policies1; 25 | global policies2; 26 | global HessianArray; 27 | global ParameterArray1; 28 | global ParameterArray2; 29 | 30 | global PGOutputPub; 31 | global PGEllaTaskIndexPub; 32 | global TranslationMsg; 33 | global RotationMsg; 34 | global Int16Msg; 35 | global nextTaskExists; 36 | 37 | rosMasterIp = 'localhost'; 38 | localhostIp = '127.0.1.1'; 39 | node = rosmatlab.node('matlab_pg_ella', rosMasterIp, 11311, 'rosIP', localhostIp); 40 | 41 | K=2; 42 | numEpisodesTilUpdate = 100; % Length of one trajectory 43 | numEpisodes = 0; 44 | numTasks = 6; 45 | dimStateSpace = 2; 46 | nextTaskExists = true; 47 | 48 | numTrajectories = 0; 49 | numTrajectoriesTilUpdate = 10; 50 | 51 | % Create a message. 52 | LinearVelPolicyMsg = rosmatlab.message('geometry_msgs/Transform', node); 53 | AngularVelPolicyMsg = rosmatlab.message('geometry_msgs/Transform', node); 54 | TranslationMsg = rosmatlab.message('geometry_msgs/Vector3', node); 55 | RotationMsg = rosmatlab.message('geometry_msgs/Quaternion', node); 56 | Int16Msg = rosmatlab.message('std_msgs/Int16', node); 57 | 58 | % Model 59 | model1.L=rand(dimStateSpace,K); % dxk, where d=2, k=1 60 | model1.S=rand(K,numTasks);%zeros(K,numTasks);%rand(K,numTasks); 61 | model1.mu_one = 0.001; 62 | model1.T = 0; % taskIndex 63 | model1.learningrate =0.0000000001;%0.000000000001; 64 | 65 | model2.L=rand(dimStateSpace,K); % dxk, where d=2, k=1 66 | model2.S=rand(K,numTasks);%zeros(K,numTasks);%rand(K,numTasks); 67 | model2.mu_one = 0.001; 68 | model2.T = 0; % taskIndex 69 | model2.learningrate = 0.000000000001;%0.0000000000001; 70 | 71 | % Policies 72 | for taskIndex=1:numTasks 73 | pg_Param1(taskIndex).param.N=dimStateSpace; 74 | pg_Param1(taskIndex).param.M=1; 75 | pg_Param1(taskIndex).param.gamma = 0.9; 76 | 77 | pg_Param2(taskIndex).param.N=dimStateSpace; 78 | pg_Param2(taskIndex).param.M=1; 79 | pg_Param2(taskIndex).param.gamma = 0.9; 80 | 81 | policies1(taskIndex).policy.theta.k = rand(dimStateSpace,1)-.5; 82 | policies2(taskIndex).policy.theta.k = rand(dimStateSpace,1)-.5; 83 | policies1(taskIndex).policy.theta.sigma = rand(); 84 | policies1(taskIndex).policy.type = 3; 85 | policies2(taskIndex).policy.theta.sigma = rand(); 86 | policies2(taskIndex).policy.type = 3; 87 | 88 | HessianArray(taskIndex).D = eye(2,2); 89 | ParameterArray1(taskIndex).alpha = zeros(2,1); 90 | ParameterArray2(taskIndex).apha = zeros(2,1); 91 | end 92 | 93 | disp('policies1(1:5).policy.theta.k') 94 | disp(horzcat(policies1(1).policy.theta.k, policies1(2).policy.theta.k,policies1(3).policy.theta.k, policies1(4).policy.theta.k, policies1(5).policy.theta.k)); 95 | disp('policies2(1:5).policy.theta.k') 96 | disp(horzcat(policies2(1).policy.theta.k, policies2(2).policy.theta.k,policies2(3).policy.theta.k,policies2(4).policy.theta.k,policies2(5).policy.theta.k)); 97 | 98 | 99 | pg_Param1(1).learningrate = 0.000001; 100 | pg_Param2(1).learningrate = 0.0000001; 101 | pg_Param1(2).learningrate = 0.000001; 102 | pg_Param2(2).learningrate = 0.000001; 103 | pg_Param1(3).learningrate = 0.0000001; 104 | pg_Param2(3).learningrate = 0.000001; 105 | pg_Param1(4).learningrate = 0.0000001; 106 | pg_Param2(4).learningrate = 0.00001; 107 | pg_Param1(5).learningrate = 0.0000001; 108 | pg_Param2(5).learningrate = 0.00001; 109 | pg_Param1(6).learningrate = 0.000001; 110 | pg_Param2(6).learningrate = 0.000001; 111 | 112 | States1 = zeros(2,1); 113 | States2 = zeros(2,1); 114 | Rewards = 0; 115 | Actions1 = 0; 116 | Actions2 = 0; 117 | 118 | 119 | % Subscribers 120 | PGInputSub = node.addSubscriber('/pg_ella_input', 'geometry_msgs/Transform', 1); 121 | PGInputSub.setOnNewMessageListeners({@pg_ellainputCallback2}); 122 | 123 | % Publisher 124 | PGOutputPub = node.addPublisher('/pg_ella_output', 'geometry_msgs/Transform'); 125 | PGEllaTaskIndexPub = node.addPublisher('/pg_ella_task_index', 'std_msgs/Int16'); 126 | 127 | %PGInitPolicySub = node.addSubscriber('/pg_ella_init_policy', 'geometry_msgs/Transform', 50); 128 | %PGInitPolicySub.setOnNewMessageListeners({@pg_ellainitpolicyCallback}); 129 | 130 | taskIndex_ = 1; 131 | sendNextTask(taskIndex_); 132 | 133 | % To shutdown: node.Node.shutdown(); 134 | -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/pg_ellainitpolicyCallback.m: -------------------------------------------------------------------------------- 1 | function pginitpolicyCallback (message) 2 | % Set initial theta. 3 | 4 | global Policy1; 5 | global Policy2; 6 | 7 | translation = message.getTranslation(); 8 | rotation = message.getRotation(); 9 | 10 | numPolicy = translation.getY(); 11 | if (numPolicy == 0) 12 | return; 13 | end 14 | 15 | if (translation.getZ() == 0) 16 | Policy1(numPolicy).theta.k = transpose([rotation.getX(), rotation.getY()]) 17 | Policy1(numPolicy).theta.sigma = translation.getX(); 18 | else 19 | Policy2(numPolicy).theta.k = transpose([rotation.getX(), rotation.getY()]) 20 | Policy2(numPolicy).theta.sigma = translation.getX(); 21 | end 22 | 23 | numPolicy 24 | Policy1(numPolicy).theta.k 25 | Policy2(numPolicy).theta.k 26 | 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/pg_ellainputCallback.m: -------------------------------------------------------------------------------- 1 | function pg_ellainputCallback (message) 2 | % Collect trajectories, and compute new policy when episode 3 | % ends. 4 | 5 | global States1; 6 | global States2; 7 | global Rewards; 8 | global Actions1; 9 | global Actions2; 10 | global numEpisodes; 11 | global numEpisodesTilUpdate; 12 | global numLearningRates; 13 | global numTasks; 14 | 15 | global model1; 16 | global model2; 17 | global taskIndex_; 18 | global pg_Param1; 19 | global pg_Param2; 20 | %global policies; 21 | global policies1; 22 | global policies2; 23 | global HessianArray; 24 | global ParameterArray1; 25 | global ParameterArray2; 26 | 27 | global nextTaskExists; 28 | 29 | if (nextTaskExists) 30 | 31 | translation = message.getTranslation(); 32 | rotation = message.getRotation(); 33 | 34 | Actions1(1,end+1) = translation.getX(); % linear_vel 35 | Actions2(1,end+1) = translation.getY(); % angular_vel 36 | Rewards(1,end+1) = translation.getZ(); % reward 37 | 38 | % linear_vel = k1*d + k2*|a| 39 | States1(1,end+1) = rotation.getX(); 40 | States1(2,end) = abs(rotation.getY()); 41 | 42 | % angular_vel = k1/d + k2*a 43 | States2(1,end+1) = rotation.getX(); 44 | States2(2,end) = rotation.getY(); 45 | 46 | if (rotation.getW() == 1) 47 | % Terminal state 48 | 49 | numEpisodes = numEpisodes+1; 50 | 51 | if (numEpisodes == numEpisodesTilUpdate) 52 | % Update policies 53 | numEpisodes = 0; 54 | 55 | data1.u = Actions1(2:end); 56 | data2.u = Actions2(2:end); 57 | data1.r = Rewards(2:end); 58 | data2.r = Rewards(2:end); 59 | 60 | data1.x = States1(:, 2:end); 61 | data2.x = States2(:, 2:end); 62 | 63 | % Compute new policy 64 | [model1, HessianArray, ParameterArray1]=updatePGELLA(data1, model1, taskIndex_, pg_Param1, policies1, HessianArray, ParameterArray1); 65 | [model2, HessianArray, ParameterArray2]=updatePGELLA(data2, model2, taskIndex_, pg_Param2, policies2, HessianArray, ParameterArray2); 66 | 67 | % Change to next task 68 | taskIndex_ = taskIndex_+1; 69 | 70 | if (taskIndex_ < numTasks) 71 | sendNextTask(taskIndex_); 72 | else 73 | sendPolicies(); 74 | nextTaskExists = false; 75 | end 76 | 77 | % Clear trajectory 78 | States1 = zeros(2,1); 79 | States2 = zeros(2,1); 80 | Actions1 = 0; 81 | Actions2 = 0; 82 | Rewards = 0; 83 | 84 | end 85 | end 86 | 87 | end 88 | end 89 | -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/sendNextTask.m: -------------------------------------------------------------------------------- 1 | % Publish the next taskIndex_. 2 | function sendNextTask(taskIndex) 3 | global Int16Msg; 4 | global PGEllaTaskIndexPub; 5 | 6 | Int16Msg.setData(taskIndex); 7 | PGEllaTaskIndexPub.publish(Int16Msg); 8 | end -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/sendPolicies.m: -------------------------------------------------------------------------------- 1 | function sendPolicies() 2 | global PGOutputPub; 3 | global LinearVelPolicyMsg; 4 | global TranslationMsg; 5 | global RotationMsg; 6 | global numTasks; 7 | global model1; 8 | global model2; 9 | 10 | 11 | for taskIndex=1:model1(1).T 12 | [policy1]=performActionPGELLA(model1, taskIndex); 13 | [policy2]=performActionPGELLA(model2, taskIndex); 14 | 15 | % publish to ROS; send new policy 16 | TranslationMsg.setX(policy1.theta.sigma); 17 | TranslationMsg.setY(taskIndex); 18 | TranslationMsg.setZ(0); % linear_vel: msg.translation.z = 0 19 | RotationMsg.setX(policy1.theta.k(1,:)); 20 | RotationMsg.setY(policy1.theta.k(2,:)); 21 | LinearVelPolicyMsg.setRotation(RotationMsg); 22 | LinearVelPolicyMsg.setTranslation(TranslationMsg); 23 | PGOutputPub.publish(LinearVelPolicyMsg); 24 | 25 | TranslationMsg.setX(policy2.theta.sigma); 26 | TranslationMsg.setY(taskIndex); 27 | TranslationMsg.setZ(1); % angular_vel: msg.translation.z = 1 28 | RotationMsg.setX(policy2.theta.k(1,:)); 29 | RotationMsg.setY(policy2.theta.k(2,:)); 30 | LinearVelPolicyMsg.setRotation(RotationMsg); 31 | LinearVelPolicyMsg.setTranslation(TranslationMsg); 32 | PGOutputPub.publish(LinearVelPolicyMsg); 33 | end 34 | 35 | disp(['Task # = ', model1(1).T]) 36 | 37 | disp('model1.L') 38 | disp(model1.L) 39 | disp('model1.S') 40 | disp(model1.S) 41 | 42 | disp('model2.L') 43 | disp(model2.L) 44 | disp('model2.S') 45 | disp(model2.S) 46 | 47 | end -------------------------------------------------------------------------------- /MATLAB/PG-ELLA/testPGELLA.m: -------------------------------------------------------------------------------- 1 | data.x=rand(2,10); 2 | data.u=rand(1,10); 3 | data.r=rand(1,10); 4 | 5 | K=1; 6 | T=10; % number of tasks 7 | model.L=rand(2,K); % dxk, where d=2, k=1 8 | model.S=rand(K,T); 9 | model.mu_one = 0.001; 10 | model.T = 0; % number of tasks 11 | model.learningrate = 0.1; 12 | 13 | for i=1:10 14 | pg_Param(i).learningrate = 0.00001; 15 | pg_Param(i).param.N=2; 16 | pg_Param(i).param.M=1; 17 | pg_Param(i).param.gamma = 0.9; 18 | 19 | policies(i).policy.theta.k = rand(2,1); 20 | policies(i).policy.theta.sigma = rand(); 21 | policies(i).policy.type = 3; 22 | HessianArray(i).D=eye(2,2); 23 | ParameterArray(i).alpha=zeros(2,1); 24 | end 25 | 26 | for taskIndex=1:10 27 | [model, HessianArray, ParameterArray]=updatePGELLA(data, model, taskIndex ,pg_Param, policies, HessianArray, ParameterArray) 28 | end 29 | 30 | model(1).L 31 | model.S(:,2) 32 | 33 | [policy]=performActionPGELLA(model,1) -------------------------------------------------------------------------------- /MATLAB/PG/Library/DlogPiDTheta.m: -------------------------------------------------------------------------------- 1 | function der = DlogPiDTheta(policy, x,u,param) 2 | 3 | %global N M 4 | N=param.N; 5 | M=param.M; 6 | k=zeros(N,1); 7 | xx=zeros(N,1); 8 | der=zeros(N,1); 9 | der(N+1,1)=0; 10 | %if(policy.type == 1) 11 | % der = zeros(N*(M-1),1); 12 | %if(u==M) 13 | % actions = selDecBor(x,1):selDecBor(x,M-1); 14 | % der(actions,1) = - 1 / (1 - sum(policy.theta(actions))); 15 | % else 16 | % der(selDecBor(x,u),1) = 1 / policy.theta(selDecBor(x,u)); 17 | % end; 18 | %elseif(policy.type == 2) 19 | % der = zeros(N*M,1); 20 | % for i=1:M 21 | % der(selGibbs(x,i),1) = -pi_theta(policy, x, i)/policy.eps; 22 | % end; 23 | % der(selGibbs(x,u)) = (1 - pi_theta(policy, x, u))/policy.eps; 24 | %if(policy.type == 3) 25 | 26 | sigma = max(policy.theta.sigma,0.00001); 27 | %tempTheta=policy.theta.k; 28 | %for l=1:length(tempTheta) 29 | % k(l,:)=tempTheta(l,:); 30 | % xx(l,:)=x(l,:); 31 | %end 32 | k(1:N,1) = policy.theta.k(1:N); 33 | xx(1:N,1) = x(1:N); 34 | der(1:N,1) = (u-k'*xx)*xx/(sigma^2); 35 | der(N+1,1) = ((u-k'*xx)^2-sigma^2)/(sigma^3); 36 | %end; 37 | -------------------------------------------------------------------------------- /MATLAB/PG/Library/episodicREINFORCE.m: -------------------------------------------------------------------------------- 1 | function [dJdtheta]= episodicREINFORCE(policy, data, param) 2 | 3 | N=param.N; 4 | gamma=param.gamma; 5 | %dJdtheta = zeros(size(DlogPiDTheta(policy,1,1,param))); 6 | dJdtheta = zeros(size(DlogPiDTheta(policy,ones(N,1),ones(param.M,1),param))); 7 | 8 | for Trials = 1 : max(size(data)) 9 | %dSumPi = zeros(size(DlogPiDTheta(policy,1,1,param))); 10 | dSumPi = zeros(size(DlogPiDTheta(policy,ones(N,1),ones(param.M,1),param))); 11 | sumR = 0; 12 | 13 | for Steps = 1 : max(size(data(Trials).u)) 14 | dSumPi = dSumPi + DlogPiDTheta(policy, data(Trials).x(:,Steps), data(Trials).u(Steps),param); 15 | sumR = sumR + gamma^(Steps-1)*data(Trials).r(Steps); 16 | end; 17 | 18 | dJdtheta = dJdtheta + dSumPi * sumR; 19 | end; 20 | 21 | 22 | if(gamma==1) 23 | dJdtheta = dJdtheta / i; 24 | else 25 | dJdtheta = (1-gamma)*dJdtheta / max(size(data)); 26 | end; -------------------------------------------------------------------------------- /MATLAB/PG/launch_pg.m: -------------------------------------------------------------------------------- 1 | % Copyright 2013 The MathWorks, Inc. 2 | 3 | % Set IP address of the host where ROS master is running. 4 | global rosMasterIp; 5 | rosMasterIp = 'localhost';%'158.130.108.238'; 6 | 7 | % Set IP address of the local host. 8 | global localhostIp; 9 | localhostIp = '127.0.1.1'; 10 | 11 | if ~exist(fullfile(matlabroot, 'toolbox', 'psp', 'rosmatlab'), 'dir') 12 | % Abort if ROS I/O Package is not installed. 13 | errordlg(['You must install ROS I/O Package before running this demo. ',... 14 | 'Please visit www.mathworks.com/ros to download ROS I/O Package.'], ... 15 | 'ROS I/O Package Not Found'); 16 | else 17 | % Launch Turtlebot Controller Demo. 18 | turtlebot_control; 19 | end -------------------------------------------------------------------------------- /MATLAB/PG/pg_startup.m: -------------------------------------------------------------------------------- 1 | global LinearVelPolicyMsg; 2 | global AngularVelPolicyMsg; 3 | global States1; 4 | global States2; 5 | global Actions1; 6 | global Actions2; 7 | global Rewards; 8 | global Policy1; 9 | global Policy2; 10 | global Param; 11 | global LearningRate1; 12 | global LearningRate2; 13 | global numLearningRates; 14 | global numEpisodesTilUpdate; 15 | global numEpisodes; 16 | global PGOutputPub; 17 | global TranslationMsg; 18 | global RotationMsg; 19 | 20 | rosMasterIp = 'localhost'; 21 | localhostIp = '127.0.1.1'; 22 | node = rosmatlab.node('MATLAB_PG', rosMasterIp, 11311, 'rosIP', localhostIp); 23 | 24 | numEpisodesTilUpdate = 1; 25 | numEpisodes = 0; 26 | 27 | % Publishers 28 | PGOutputPub = node.addPublisher('/pg_output', 'geometry_msgs/Transform'); 29 | 30 | % Create a message. 31 | LinearVelPolicyMsg = rosmatlab.message('geometry_msgs/Transform', node); 32 | AngularVelPolicyMsg = rosmatlab.message('geometry_msgs/Transform', node); 33 | TranslationMsg = rosmatlab.message('geometry_msgs/Vector3', node); 34 | RotationMsg = rosmatlab.message('geometry_msgs/Quaternion', node); 35 | 36 | % Learning rates 37 | LearningRate1 = [0.00000001, 0.0000001, 0.000001, 0.00001];%, 0.0001, 0.001]; 38 | LearningRate2 = [0.00000001, 0.0000001, 0.000001, 0.00001];%, 0.0001, 0.001]; 39 | numLearningRates = length(LearningRate2); 40 | 41 | for i=1:numLearningRates 42 | Policy1(i).theta.k = zeros(2,1); 43 | Policy1(i).theta.sigma = rand(); 44 | Policy1(i).type = 3; 45 | 46 | Policy2(i).theta.k = zeros(2,1); 47 | Policy2(i).theta.sigma = rand(); 48 | Policy2(i).type = 3; 49 | end 50 | 51 | % Parameters for episodicREINFORCE 52 | Param.N = 2; 53 | Param.M = 1; 54 | Param.gamma = 0.9; 55 | 56 | States1 = zeros(2,1); 57 | States2 = zeros(2,1); 58 | Rewards = 0; 59 | Actions1 = 0; 60 | Actions2 = 0; 61 | 62 | % Subscribers 63 | PGInputSub = node.addSubscriber('/pg_input', 'geometry_msgs/Transform', 1); 64 | PGInputSub.setOnNewMessageListeners({@pginputCallback}); 65 | 66 | PGInitPolicySub = node.addSubscriber('/pg_init_policy', 'geometry_msgs/Transform', 50); 67 | PGInitPolicySub.setOnNewMessageListeners({@pginitpolicyCallback}); 68 | 69 | %node.setOnShutdownListener(@preShutdownTask); 70 | %node.setOnShutdownCompleteListener(@postShutdownTask); 71 | 72 | % To shutdown: node.Node.shutdown(); 73 | -------------------------------------------------------------------------------- /MATLAB/PG/pginitpolicyCallback.m: -------------------------------------------------------------------------------- 1 | function pginitpolicyCallback (message) 2 | % Set initial theta. 3 | 4 | global Policy1; 5 | global Policy2; 6 | 7 | translation = message.getTranslation(); 8 | rotation = message.getRotation(); 9 | 10 | numPolicy = translation.getY(); 11 | if (numPolicy == 0) 12 | return; 13 | end 14 | 15 | if (translation.getZ() == 0) 16 | Policy1(numPolicy).theta.k = transpose([rotation.getX(), rotation.getY()]) 17 | Policy1(numPolicy).theta.sigma = translation.getX(); 18 | else 19 | Policy2(numPolicy).theta.k = transpose([rotation.getX(), rotation.getY()]) 20 | Policy2(numPolicy).theta.sigma = translation.getX(); 21 | end 22 | 23 | numPolicy 24 | Policy1(numPolicy).theta.k 25 | Policy2(numPolicy).theta.k 26 | 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/PG/pginputCallback.m: -------------------------------------------------------------------------------- 1 | function pginputCallback (message) 2 | % Collect trajectories, and compute new policy when episode 3 | % ends. 4 | 5 | global States1; 6 | global States2; 7 | global Rewards; 8 | global Actions1; 9 | global Actions2; 10 | global Policy1; 11 | global Policy2; 12 | global Param; 13 | global LearningRate1; 14 | global LearningRate2; 15 | global numEpisodes; 16 | global numEpisodesTilUpdate; 17 | global numLearningRates; 18 | global PGOutputPub; 19 | global LinearVelPolicyMsg; 20 | global TranslationMsg; 21 | global RotationMsg; 22 | 23 | translation = message.getTranslation(); 24 | rotation = message.getRotation(); 25 | 26 | Actions1(1,end+1) = translation.getX(); % linear_vel 27 | Actions2(1,end+1) = translation.getY(); % angular_vel 28 | Rewards(1,end+1) = translation.getZ(); % reward 29 | 30 | % linear_vel = k1*d + k2*|a| 31 | States1(1,end+1) = rotation.getX(); 32 | States1(2,end) = abs(rotation.getY()); 33 | 34 | % angular_vel = k1/d + k2*a 35 | States2(1,end+1) = rotation.getX(); 36 | States2(2,end) = rotation.getY();; 37 | 38 | if (rotation.getW() == 1) 39 | % Terminal state 40 | 41 | numEpisodes = numEpisodes+1; 42 | 43 | if (numEpisodes == numEpisodesTilUpdate) 44 | % Update policies 45 | numEpisodes = 0; 46 | 47 | data1.u = Actions1(2:end); 48 | data2.u = Actions2(2:end); 49 | data1.r = Rewards(2:end); 50 | data2.r = Rewards(2:end); 51 | 52 | data1.x = States1(:, 2:end); 53 | data2.x = States2(:, 2:end); 54 | 55 | for i=1:numLearningRates 56 | [dJdtheta]=episodicREINFORCE(Policy1(i), data1, Param); 57 | Policy1(i).theta.k = Policy1(i).theta.k + LearningRate1(i) * dJdtheta(1:Param.N,1); 58 | Policy1(i).theta.sigma = Policy1(i).theta.sigma + LearningRate1(i)*dJdtheta(Param.N+1,1) * Policy1(i).theta.sigma^2; 59 | 60 | [dJdtheta]=episodicREINFORCE(Policy2(i), data2, Param); 61 | Policy2(i).theta.k = Policy2(i).theta.k + LearningRate2(i) * dJdtheta(1:Param.N,1); 62 | Policy2(i).theta.sigma = Policy2(i).theta.sigma + LearningRate2(i)*dJdtheta(Param.N+1,1) * Policy2(i).theta.sigma^2; 63 | end 64 | 65 | % publish to ROS; send new policy 66 | %translation = rosmatlab.message('geometry_msgs/Vector3', node); 67 | %rotation = rosmatlab.message('geometry_msgs/Quaternion', node); 68 | 69 | for i=1:numLearningRates 70 | 71 | TranslationMsg.setX(Policy1(i).theta.sigma); 72 | TranslationMsg.setY(i); 73 | TranslationMsg.setZ(0); % linear_vel: msg.translation.z = 0 74 | RotationMsg.setX(Policy1(i).theta.k(1,:)); 75 | RotationMsg.setY(Policy1(i).theta.k(2,:)); 76 | LinearVelPolicyMsg.setRotation(RotationMsg); 77 | LinearVelPolicyMsg.setTranslation(TranslationMsg); 78 | PGOutputPub.publish(LinearVelPolicyMsg); 79 | 80 | TranslationMsg.setX(Policy2(i).theta.sigma); 81 | TranslationMsg.setY(i); 82 | TranslationMsg.setZ(1); % angular_vel: msg.translation.z = 1 83 | RotationMsg.setX(Policy2(i).theta.k(1,:)); 84 | RotationMsg.setY(Policy2(i).theta.k(2,:)); 85 | LinearVelPolicyMsg.setRotation(RotationMsg); 86 | LinearVelPolicyMsg.setTranslation(TranslationMsg); 87 | PGOutputPub.publish(LinearVelPolicyMsg); 88 | 89 | if (i == 3) 90 | disp(Policy1(i).theta.k) 91 | elseif (i== 2) 92 | disp(Policy2(i).theta.k) 93 | end 94 | 95 | end 96 | 97 | % Clear trajectory 98 | States1 = zeros(2,1); 99 | States2 = zeros(2,1); 100 | Actions1 = 0; 101 | Actions2 = 0; 102 | Rewards = 0; 103 | end 104 | end 105 | 106 | 107 | end 108 | -------------------------------------------------------------------------------- /MATLAB/PG/testPG.m: -------------------------------------------------------------------------------- 1 | 2 | % Main 3 | h.States = zeros(3,1); 4 | h.Rewards = 0; 5 | h.Actions1 = 0; 6 | h.Actions2 = 0; 7 | 8 | h.Policy1.theta.k = zeros(3,1); 9 | h.Policy1.theta.sigma = rand(); 10 | h.Policy1.type = 3; 11 | 12 | h.Policy2.theta.sigma = 0; 13 | h.Policy2.theta.k = zeros(3,1); 14 | h.Policy2.type = 3; 15 | 16 | h.Param.N = 3; 17 | h.Param.M = 1; 18 | h.Param.gamma = 0.9; 19 | 20 | h.LearningRate = 0.1; 21 | 22 | % pginputCallback 23 | for i=1:10 24 | h.Actions1(1,end+1)=i; 25 | h.Actions2(1,end+1)=i; 26 | 27 | h.Rewards(1,end+1)=i; 28 | 29 | h.States(1,end+1) = i; 30 | h.States(2,end) = i; 31 | h.States(3,end) = i; 32 | %h.States(4,end) = 1; 33 | end 34 | 35 | % Terminal state 36 | data1.u = h.Actions1(2:end); 37 | data2.u = h.Actions2(2:end); 38 | data1.r = h.Rewards(2:end); 39 | data2.u = h.Rewards(2:end); 40 | 41 | data1.x = h.States(:, 2:end); 42 | data2.x = h.States(:, 2:end); 43 | 44 | [dJdtheta]=episodicREINFORCE(h.Policy1, data1, h.Param); 45 | h.Policy1.theta.k = h.Policy1.theta.k + h.LearningRate * dJdtheta(1:h.Param.N,1); 46 | h.Policy1.theta.sigma = h.Policy1.theta.sigma + h.LearningRate*dJdtheta(h.Param.N+1,1) * h.Policy1.theta.sigma^2; 47 | 48 | h.Policy1.theta.k 49 | h.Policy1.theta.sigma 50 | 51 | for i=1:size(data,2) 52 | end 53 | %data.x=rand(4,10); 54 | %data.u=rand(1,10); 55 | %data.r=rand(1,10); 56 | %policy.theta.k(1) = 2; 57 | 58 | %param.N = 1; 59 | %param.M = 4; 60 | 61 | %param.gamma = 0.9; 62 | 63 | %[dJdtheta]=episodicREINFORCE(policy,data,param); 64 | 65 | %policy.theta.k=policy.theta.k+learningrate*dJdtheta(1:param.N,1); 66 | %policy.theta.sigma=policy.theta.sigma+learningrate*dJdtheta(param.N+1,1)*policy.theta.sigma^2; 67 | 68 | 69 | %DlogPiDTheta(policy,1,1,param) 70 | 71 | %numIterations=100; 72 | %for i=1:numIterations 73 | 74 | %[dJdtheta]=episodicREINFORCE(policy,data,param); 75 | %policy.theta.k=policy.theta.k+learningrate*dJdtheta(1:param.N,1); 76 | %policy.theta.sigma=policy.theta.sigma+learningrate*dJdtheta(param.N+1,1)*policy.theta.sigma^2; 77 | 78 | %end -------------------------------------------------------------------------------- /lilee_follower/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package turtlebot_follower 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.2.5 (2014-03-25) 6 | ------------------ 7 | 8 | 2.2.4 (2013-10-14) 9 | ------------------ 10 | 11 | 2.2.3 (2013-09-27) 12 | ------------------ 13 | 14 | 2.2.2 (2013-09-26) 15 | ------------------ 16 | * Use service files to turtlebot_msgs. 17 | 18 | 2.2.1 (2013-09-23) 19 | ------------------ 20 | 21 | 2.2.0 (2013-08-30) 22 | ------------------ 23 | * Rename include launchers to *.launch.xml. 24 | * Changelogs at package level. 25 | 26 | 2.1.x - hydro, unstable 27 | ======================= 28 | 29 | 2.1.1 (2013-08-09) 30 | ------------------ 31 | * Fix namespace issues 32 | * Rationalize the use of velocity smoother: remap properly robot_cmd_vel, add comments, and avoid meaningless topic names 33 | 34 | 2.1.0 (2013-07-19) 35 | ------------------ 36 | * Catkinized 37 | * Unexistent include dir removed from package description 38 | 39 | 40 | Previous versions, bugfixing 41 | ============================ 42 | 43 | Available in ROS wiki: http://ros.org/wiki/turtlebot_apps/ChangeList 44 | -------------------------------------------------------------------------------- /lilee_follower/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lilee_follower) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS nodelet pcl_ros roscpp visualization_msgs turtlebot_msgs dynamic_reconfigure) 6 | find_package(Boost REQUIRED) 7 | find_package(PCL) 8 | 9 | generate_dynamic_reconfigure_options(cfg/Follower.cfg) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS 13 | LIBRARIES ${PROJECT_NAME} 14 | CATKIN_DEPENDS nodelet pcl_ros roscpp visualization_msgs turtlebot_msgs dynamic_reconfigure 15 | DEPENDS Boost PCL 16 | ) 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | 22 | ## Specify additional locations of header files 23 | include_directories( 24 | include 25 | ${catkin_INCLUDE_DIRS} 26 | ${PCL_INCLUDE_DIRS} 27 | ${Boost_INCLUDE_DIRS} 28 | ) 29 | 30 | ## Declare a cpp library 31 | add_library(${PROJECT_NAME} src/lilee_follower.cpp) 32 | 33 | add_dependencies(${PROJECT_NAME} turtlebot_msgs_gencpp) 34 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 35 | 36 | 37 | ## Specify libraries to link a library or executable target against 38 | target_link_libraries(${PROJECT_NAME} 39 | ${catkin_LIBRARIES} 40 | ${PCL_LIBRARIES} 41 | ) 42 | 43 | 44 | ############# 45 | ## Install ## 46 | ############# 47 | 48 | ## Mark executables and/or libraries for installation 49 | install(TARGETS ${PROJECT_NAME} 50 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | ) 54 | 55 | install(TARGETS lilee_follower 56 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 59 | ) 60 | 61 | ## Mark all other required files for installation 62 | install(DIRECTORY sim 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 64 | ) 65 | 66 | install(DIRECTORY launch 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 68 | ) 69 | 70 | install(DIRECTORY param 71 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 72 | ) 73 | 74 | install(DIRECTORY plugins 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 76 | ) 77 | 78 | ############# 79 | ## Testing ## 80 | ############# 81 | 82 | ## Add gtest based cpp test target and link libraries 83 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turtlebot_follower.cpp) 84 | # if(TARGET ${PROJECT_NAME}-test) 85 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 86 | # endif() 87 | 88 | ## Add folders to be run by python nosetests 89 | # catkin_add_nosetests(test) 90 | -------------------------------------------------------------------------------- /lilee_follower/cfg/.gitignore: -------------------------------------------------------------------------------- 1 | /cpp 2 | -------------------------------------------------------------------------------- /lilee_follower/cfg/Follower.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2009, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | 35 | PACKAGE='lilee_follower' 36 | 37 | import math 38 | 39 | from dynamic_reconfigure.parameter_generator_catkin import * 40 | 41 | gen = ParameterGenerator() 42 | gen.add("min_x", double_t, 0, "The minimum x position of the points in the box.", -0.20, -3.0, 3.0) 43 | gen.add("max_x", double_t, 0, "The maximum x position of the points in the box.", 0.20, -3.0, 3.0) 44 | gen.add("min_y", double_t, 0, "The minimum y position of the points in the box.", 0.10, -1.5, 3.0) 45 | gen.add("max_y", double_t, 0, "The maximum y position of the points in the box.", 0.50, -1.0, 3.0) 46 | gen.add("max_z", double_t, 0, "The maximum z position of the points in the box.", 0.8, 0.0, 10) 47 | gen.add("goal_z", double_t, 0, "The distance away from the robot to hold the centroid.", 0.6, 0.0, 3.0) 48 | gen.add("x_scale", double_t, 0, "The scaling factor for translational robot speed.", 1.0, 0.0, 3.0) 49 | gen.add("z_scale", double_t, 0, "The scaling factor for rotational robot speed.", 5.0, 0.0, 10.0) 50 | 51 | 52 | exit(gen.generate(PACKAGE, "turtlebot_follower_dynamic_reconfigure", "Follower")) 53 | -------------------------------------------------------------------------------- /lilee_follower/launch/follower.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /lilee_follower/launch/includes/safety_controller.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 11 | 12 | -------------------------------------------------------------------------------- /lilee_follower/launch/includes/velocity_smoother.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /lilee_follower/package.xml: -------------------------------------------------------------------------------- 1 | 2 | lilee_follower 3 | 2.2.5 4 | 5 | Follower for the turtlebot. Follows humans and robots around by following the centroid of a box points in front of the turtlebot. 6 | 7 | 8 | Lisa Lee 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/turtlebot_apps 13 | https://github.com/turtlebot/turtlebot_apps 14 | https://github.com/turtlebot/turtlebot_apps/issues 15 | 16 | Tony Pratkanis 17 | 18 | catkin 19 | 20 | nodelet 21 | pcl_ros 22 | roscpp 23 | visualization_msgs 24 | dynamic_reconfigure 25 | turtlebot_msgs 26 | cv_bridge 27 | sensor_msgs 28 | std_msgs 29 | image_transport 30 | 31 | nodelet 32 | pcl_ros 33 | roscpp 34 | visualization_msgs 35 | dynamic_reconfigure 36 | turtlebot_bringup 37 | turtlebot_msgs 38 | cv_bridge 39 | sensor_msgs 40 | std_msgs 41 | image_transport 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /lilee_follower/param/mux.yaml: -------------------------------------------------------------------------------- 1 | # Created on: Dec 5, 2012 2 | # Author: jorge 3 | # Configuration for subscribers to cmd_vel sources. 4 | # 5 | # Individual subscriber configuration: 6 | # name: Source name 7 | # topic: The topic that provides cmd_vel messages 8 | # timeout: Time in seconds without incoming messages to consider this topic inactive 9 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 10 | # short_desc: Short description (optional) 11 | 12 | subscribers: 13 | - name: "Safe reactive controller" 14 | topic: "input/safety_controller" 15 | timeout: 0.2 16 | priority: 10 17 | - name: "Follower" 18 | topic: "input/follower" 19 | timeout: 0.5 20 | priority: 7 21 | -------------------------------------------------------------------------------- /lilee_follower/plugins/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The turtlebot people follower node. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /lilee_follower/src/.gitignore: -------------------------------------------------------------------------------- 1 | /turtlebot_follower 2 | -------------------------------------------------------------------------------- /lilee_follower/src/lilee_follower.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "std_msgs/String.h" 11 | #include 12 | #include 13 | #include // htonl 14 | #include "lilee_reinforcement/State.h" 15 | 16 | #include "dynamic_reconfigure/server.h" 17 | #include "lilee_follower/FollowerConfig.h" 18 | 19 | namespace lilee_follower 20 | { 21 | typedef pcl::PointCloud PointCloud; 22 | /** 23 | * The turtlebot follower nodelet. Subscribes to point clouds 24 | * from the 3dsensor, processes them, and publishes command vel 25 | * messages. 26 | */ 27 | class LileeFollower : public nodelet::Nodelet 28 | { 29 | public: 30 | /*! 31 | * @brief The constructor for the follower. 32 | * Constructor for the follower. 33 | */ 34 | LileeFollower() : min_y_(0.1), max_y_(0.5), 35 | min_x_(-0.2), max_x_(0.2), 36 | max_z_(0.8), goal_z_(0.6), 37 | z_scale_(1.0), x_scale_(5.0) 38 | { 39 | 40 | } 41 | 42 | ~LileeFollower() 43 | { 44 | delete config_srv_; 45 | } 46 | 47 | private: 48 | double min_y_; /**< The minimum y position of the points in the box. */ 49 | double max_y_; /**< The maximum y position of the points in the box. */ 50 | double min_x_; /**< The minimum x position of the points in the box. */ 51 | double max_x_; /**< The maximum x position of the points in the box. */ 52 | double max_z_; /**< The maximum z position of the points in the box. */ 53 | double goal_z_; /**< The distance away from the robot to hold the centroid */ 54 | double z_scale_; /**< The scaling factor for translational robot speed */ 55 | double x_scale_; /**< The scaling factor for rotational robot speed */ 56 | bool enabled_; /**< Enable/disable following; just prevents motor commands */ 57 | float focal_length_; 58 | bool flag; 59 | 60 | // coordinate of rectangle vertices 61 | int ul_x, ul_y, ll_x, ll_y, lr_x, lr_y, ur_x, ur_y; 62 | 63 | // Service for start/stop following 64 | ros::ServiceServer switch_srv_; 65 | 66 | // Dynamic reconfigure server 67 | dynamic_reconfigure::Server* config_srv_; 68 | 69 | /*! 70 | * @brief OnInit method from node handle. 71 | * OnInit method from node handle. Sets up the parameters 72 | * and topics. 73 | */ 74 | virtual void onInit() 75 | { 76 | ROS_ERROR("lilee_follower: hello from onInit()"); 77 | ros::NodeHandle& nh = getNodeHandle(); 78 | ros::NodeHandle& private_nh = getPrivateNodeHandle(); 79 | 80 | flag = true; 81 | ul_x = ul_y = lr_x = lr_y = 0; 82 | focal_length_ = 575.8157348632812; 83 | 84 | private_nh.getParam("min_y", min_y_); 85 | private_nh.getParam("max_y", max_y_); 86 | private_nh.getParam("min_x", min_x_); 87 | private_nh.getParam("max_x", max_x_); 88 | private_nh.getParam("max_z", max_z_); 89 | private_nh.getParam("goal_z", goal_z_); 90 | private_nh.getParam("z_scale", z_scale_); 91 | private_nh.getParam("x_scale", x_scale_); 92 | private_nh.getParam("enabled", enabled_); 93 | 94 | cmdpub_ = private_nh.advertise ("cmd_vel", 1); 95 | markerpub_ = private_nh.advertise("marker",1); 96 | bboxpub_ = private_nh.advertise("bbox",1); 97 | statespub_ = nh.advertise("states",1); 98 | 99 | rectanglesub_ = nh.subscribe("/location_data", 1, &LileeFollower::rectanglecb, this); 100 | depthrectanglesub_ = nh.subscribe("/camera/depth/image_raw", 1, &LileeFollower::depthrectanglecb, this); 101 | 102 | switch_srv_ = private_nh.advertiseService("change_state", &LileeFollower::changeModeSrvCb, this); 103 | 104 | config_srv_ = new dynamic_reconfigure::Server(private_nh); 105 | dynamic_reconfigure::Server::CallbackType f = 106 | boost::bind(&LileeFollower::reconfigure, this, _1, _2); 107 | config_srv_->setCallback(f); 108 | } 109 | 110 | void reconfigure(lilee_follower::FollowerConfig &config, uint32_t level) 111 | { 112 | min_y_ = config.min_y; 113 | max_y_ = config.max_y; 114 | min_x_ = config.min_x; 115 | max_x_ = config.max_x; 116 | max_z_ = config.max_z; 117 | goal_z_ = config.goal_z; 118 | z_scale_ = config.z_scale; 119 | x_scale_ = config.x_scale; 120 | } 121 | 122 | /* Convert [c1, c2, c3, c4] into a float */ 123 | float charsToFloat(unsigned char c1, unsigned char c2, unsigned char c3, unsigned char c4, bool is_bigendian) 124 | { 125 | float f; 126 | 127 | if ( htonl(47) == 47 ) { 128 | // System is big endian 129 | if (is_bigendian) { 130 | *((char *)(&f)) = c1; 131 | *((char *)(&f)+1) = c2; 132 | *((char *)(&f)+2) = c3; 133 | *((char *)(&f)+3) = c4; 134 | } 135 | else { 136 | *((char *)(&f)) = c4; 137 | *((char *)(&f)+1) = c3; 138 | *((char *)(&f)+2) = c2; 139 | *((char *)(&f)+3) = c1; 140 | } 141 | } 142 | 143 | else { 144 | //System is little endian 145 | if (!is_bigendian) { 146 | *((char *)(&f)) = c1; 147 | *((char *)(&f)+1) = c2; 148 | *((char *)(&f)+2) = c3; 149 | *((char *)(&f)+3) = c4; 150 | } 151 | else { 152 | *((char *)(&f)) = c4; 153 | *((char *)(&f)+1) = c3; 154 | *((char *)(&f)+2) = c2; 155 | *((char *)(&f)+3) = c1; 156 | } 157 | } 158 | 159 | return f; 160 | } 161 | 162 | /*! 163 | * @brief Callback for rectangle coordinates. 164 | */ 165 | void rectanglecb(const std_msgs::String::ConstPtr& msg){ 166 | int temp_ul_x, temp_ul_y, temp_lr_x, temp_lr_y; 167 | 168 | sscanf(msg->data.c_str(), "(%d, %d),(%d, %d)", &temp_ul_x, &temp_ul_y, &temp_lr_x, &temp_lr_y); 169 | 170 | ul_x = temp_ul_x; 171 | ul_y = temp_ul_y; 172 | lr_x = temp_lr_x; 173 | lr_y = temp_lr_y; 174 | 175 | flag = false; 176 | } 177 | 178 | /*! 179 | * @brief Callback for /camera/depth/image. 180 | */ 181 | void depthrectanglecb(const sensor_msgs::ImageConstPtr& msg) 182 | { 183 | if (flag == false) { 184 | flag = true; 185 | 186 | lilee_reinforcement::State state; 187 | float x = 0.0, y = 0.0, z = 1e6, temp_x, temp_y, temp_z; 188 | unsigned int n = 0; 189 | 190 | // for each (x, y) inside rectangle 191 | for (int i = ul_x; i < lr_x; i++) for (int j = ul_y; j < lr_y; j++) 192 | { 193 | int index = j*4*(msg->width) + i*4; 194 | 195 | temp_z = charsToFloat(msg->data[index], msg->data[index+1], msg->data[index+2], msg->data[index+3], msg->is_bigendian); 196 | 197 | //If there are points, calculate the command goal. 198 | //If there are no points, simply publish a stop goal. 199 | if (!std::isnan(temp_z) && temp_z > 0.0) { 200 | 201 | temp_x = (i-320)*temp_z/focal_length_; 202 | temp_y = (j-240)*temp_z/focal_length_; 203 | 204 | //Test to ensure the point is within the aceptable box. 205 | if (-temp_y > min_y_ && -temp_y < max_y_ && temp_x < max_x_ && temp_x > min_x_ && temp_z < max_z_) 206 | { 207 | //Add the point to the totals 208 | x += temp_x; 209 | y += temp_y; 210 | 211 | z = std::min(z, temp_z); 212 | 213 | n++; 214 | } 215 | } 216 | } 217 | 218 | 219 | //If there are points, find the centroid and calculate the command goal. 220 | //If there are no points, simply publish a stop goal. 221 | if (n>100) 222 | { 223 | x /= n; 224 | y /= n; 225 | 226 | ROS_INFO("Centroid at %f %f %f with %d points", x, y, z, n); 227 | 228 | if(z > max_z_) 229 | { 230 | ROS_ERROR("No valid points detected, stopping the robot"); 231 | if (enabled_) 232 | { 233 | cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist())); 234 | } 235 | return; 236 | } 237 | 238 | publishMarker(x, y, z); 239 | 240 | if (enabled_) 241 | { 242 | geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist()); 243 | ROS_INFO("In depthrectanglecb: cmd = (linear.x = %f, angular.z = %f)", (z - goal_z_) * z_scale_, -x * x_scale_); 244 | 245 | cmd->linear.x = (z - goal_z_) * z_scale_; 246 | cmd->angular.z = -x * x_scale_; 247 | 248 | cmdpub_.publish(cmd); 249 | } 250 | } 251 | else 252 | { 253 | ROS_DEBUG("No points detected, stopping the robot"); 254 | publishMarker(x, y, z); 255 | 256 | if (enabled_) 257 | { 258 | cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist())); 259 | } 260 | } 261 | 262 | publishBbox(); 263 | } 264 | } 265 | 266 | bool changeModeSrvCb(turtlebot_msgs::SetFollowState::Request& request, 267 | turtlebot_msgs::SetFollowState::Response& response) 268 | { 269 | if ((enabled_ == true) && (request.state == request.STOPPED)) 270 | { 271 | ROS_INFO("Change mode service request: following stopped"); 272 | cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist())); 273 | enabled_ = false; 274 | } 275 | else if ((enabled_ == false) && (request.state == request.FOLLOW)) 276 | { 277 | ROS_INFO("Change mode service request: following (re)started"); 278 | enabled_ = true; 279 | } 280 | 281 | response.result = response.OK; 282 | return true; 283 | } 284 | 285 | void publishMarker(double x,double y,double z) 286 | { 287 | visualization_msgs::Marker marker; 288 | marker.header.frame_id = "/camera_rgb_optical_frame"; 289 | marker.header.stamp = ros::Time(); 290 | marker.ns = "my_namespace"; 291 | marker.id = 0; 292 | marker.type = visualization_msgs::Marker::SPHERE; 293 | marker.action = visualization_msgs::Marker::ADD; 294 | marker.pose.position.x = x; 295 | marker.pose.position.y = y; 296 | marker.pose.position.z = z; 297 | marker.pose.orientation.x = 0.0; 298 | marker.pose.orientation.y = 0.0; 299 | marker.pose.orientation.z = 0.0; 300 | marker.pose.orientation.w = 1.0; 301 | marker.scale.x = 0.2; 302 | marker.scale.y = 0.2; 303 | marker.scale.z = 0.2; 304 | marker.color.a = 1.0; 305 | marker.color.r = 1.0; 306 | marker.color.g = 0.0; 307 | marker.color.b = 0.0; 308 | //only if using a MESH_RESOURCE marker type: 309 | markerpub_.publish( marker ); 310 | } 311 | 312 | void publishBbox() 313 | { 314 | double x = (min_x_ + max_x_)/2; 315 | double y = (min_y_ + max_y_)/2; 316 | double z = (0 + max_z_)/2; 317 | 318 | double scale_x = (max_x_ - x)*2; 319 | double scale_y = (max_y_ - y)*2; 320 | double scale_z = (max_z_ - z)*2; 321 | 322 | visualization_msgs::Marker marker; 323 | marker.header.frame_id = "/camera_rgb_optical_frame"; 324 | marker.header.stamp = ros::Time(); 325 | marker.ns = "my_namespace"; 326 | marker.id = 1; 327 | marker.type = visualization_msgs::Marker::CUBE; 328 | marker.action = visualization_msgs::Marker::ADD; 329 | marker.pose.position.x = x; 330 | marker.pose.position.y = -y; 331 | marker.pose.position.z = z; 332 | marker.pose.orientation.x = 0.0; 333 | marker.pose.orientation.y = 0.0; 334 | marker.pose.orientation.z = 0.0; 335 | marker.pose.orientation.w = 1.0; 336 | marker.scale.x = scale_x; 337 | marker.scale.y = scale_y; 338 | marker.scale.z = scale_z; 339 | marker.color.a = 0.5; 340 | marker.color.r = 0.0; 341 | marker.color.g = 1.0; 342 | marker.color.b = 0.0; 343 | //only if using a MESH_RESOURCE marker type: 344 | bboxpub_.publish( marker ); 345 | } 346 | 347 | ros::Subscriber sub_; 348 | ros::Subscriber centersub_; 349 | ros::Subscriber depthsub_; 350 | ros::Subscriber rectanglesub_; 351 | ros::Subscriber depthrectanglesub_; 352 | 353 | ros::Publisher cmdpub_; 354 | ros::Publisher markerpub_; 355 | ros::Publisher bboxpub_; 356 | ros::Publisher statespub_; 357 | }; 358 | 359 | PLUGINLIB_DECLARE_CLASS(lilee_follower, LileeFollower, lilee_follower::LileeFollower, nodelet::Nodelet); 360 | 361 | } 362 | -------------------------------------------------------------------------------- /lilee_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lilee_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp rospy) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 28 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 29 | ## pulled in transitively but can be declared for certainty nonetheless: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################### 70 | ## catkin specific configuration ## 71 | ################################### 72 | ## The catkin_package macro generates cmake config files for your package 73 | ## Declare things to be passed to dependent projects 74 | ## INCLUDE_DIRS: uncomment this if you package contains header files 75 | ## LIBRARIES: libraries you create in this project that dependent projects also need 76 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 77 | ## DEPENDS: system dependencies of this project that dependent projects also need 78 | catkin_package( 79 | # INCLUDE_DIRS include 80 | # LIBRARIES lilee_gazebo 81 | # CATKIN_DEPENDS other_catkin_pkg 82 | # DEPENDS system_lib 83 | ) 84 | 85 | ########### 86 | ## Build ## 87 | ########### 88 | 89 | ## Specify additional locations of header files 90 | ## Your package locations should be listed before other locations 91 | include_directories(include ${catkin_INCLUDE_DIRS}) 92 | 93 | ## Declare a cpp library 94 | # add_library(lilee_gazebo 95 | # src/${PROJECT_NAME}/lilee_gazebo.cpp 96 | # ) 97 | 98 | ## Declare a cpp executable 99 | # add_executable(lilee_gazebo_node src/lilee_gazebo_node.cpp) 100 | 101 | ## Add cmake target dependencies of the executable/library 102 | ## as an example, message headers may need to be generated before nodes 103 | # add_dependencies(lilee_gazebo_node lilee_gazebo_generate_messages_cpp) 104 | 105 | ## Specify libraries to link a library or executable target against 106 | # target_link_libraries(lilee_gazebo_node 107 | # ${catkin_LIBRARIES} 108 | # ) 109 | 110 | ############# 111 | ## Install ## 112 | ############# 113 | 114 | # all install targets should use catkin DESTINATION variables 115 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 116 | 117 | ## Mark executable scripts (Python etc.) for installation 118 | ## in contrast to setup.py, you can choose the destination 119 | # install(PROGRAMS 120 | # scripts/my_python_script 121 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 122 | # ) 123 | 124 | ## Mark executables and/or libraries for installation 125 | # install(TARGETS lilee_gazebo lilee_gazebo_node 126 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 128 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 129 | # ) 130 | 131 | ## Mark cpp header files for installation 132 | # install(DIRECTORY include/${PROJECT_NAME}/ 133 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | # FILES_MATCHING PATTERN "*.h" 135 | # PATTERN ".svn" EXCLUDE 136 | # ) 137 | 138 | ## Mark other files for installation (e.g. launch and bag files, etc.) 139 | # install(FILES 140 | # # myfile1 141 | # # myfile2 142 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 143 | # ) 144 | 145 | ############# 146 | ## Testing ## 147 | ############# 148 | 149 | ## Add gtest based cpp test target and link libraries 150 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lilee_gazebo.cpp) 151 | # if(TARGET ${PROJECT_NAME}-test) 152 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 153 | # endif() 154 | 155 | ## Add folders to be run by python nosetests 156 | # catkin_add_nosetests(test) 157 | 158 | -------------------------------------------------------------------------------- /lilee_gazebo/cmake/lilee_gazeboConfig-version.cmake: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkgConfig-version.cmake.in 2 | set(PACKAGE_VERSION "2.1.1") 3 | 4 | set(PACKAGE_VERSION_EXACT False) 5 | set(PACKAGE_VERSION_COMPATIBLE False) 6 | 7 | if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") 8 | set(PACKAGE_VERSION_EXACT True) 9 | set(PACKAGE_VERSION_COMPATIBLE True) 10 | endif() 11 | 12 | if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") 13 | set(PACKAGE_VERSION_COMPATIBLE True) 14 | endif() 15 | -------------------------------------------------------------------------------- /lilee_gazebo/cmake/lilee_gazeboConfig.cmake: -------------------------------------------------------------------------------- 1 | # generated from catkin/cmake/template/pkgConfig.cmake.in 2 | 3 | # append elements to a list and remove existing duplicates from the list 4 | # copied from catkin/cmake/list_append_deduplicate.cmake to keep pkgConfig 5 | # self contained 6 | macro(_list_append_deduplicate listname) 7 | if(NOT "${ARGN}" STREQUAL "") 8 | if(${listname}) 9 | list(REMOVE_ITEM ${listname} ${ARGN}) 10 | endif() 11 | list(APPEND ${listname} ${ARGN}) 12 | endif() 13 | endmacro() 14 | 15 | # append elements to a list if they are not already in the list 16 | # copied from catkin/cmake/list_append_unique.cmake to keep pkgConfig 17 | # self contained 18 | macro(_list_append_unique listname) 19 | foreach(_item ${ARGN}) 20 | list(FIND ${listname} ${_item} _index) 21 | if(_index EQUAL -1) 22 | list(APPEND ${listname} ${_item}) 23 | endif() 24 | endforeach() 25 | endmacro() 26 | 27 | # pack a list of libraries with optional build configuration keywords 28 | # copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig 29 | # self contained 30 | macro(_pack_libraries_with_build_configuration VAR) 31 | set(${VAR} "") 32 | set(_argn ${ARGN}) 33 | list(LENGTH _argn _count) 34 | set(_index 0) 35 | while(${_index} LESS ${_count}) 36 | list(GET _argn ${_index} lib) 37 | if("${lib}" MATCHES "^debug|optimized|general$") 38 | math(EXPR _index "${_index} + 1") 39 | if(${_index} EQUAL ${_count}) 40 | message(FATAL_ERROR "_pack_libraries_with_build_configuration() the list of libraries '${ARGN}' ends with '${lib}' which is a build configuration keyword and must be followed by a library") 41 | endif() 42 | list(GET _argn ${_index} library) 43 | list(APPEND ${VAR} "${lib}${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}${library}") 44 | else() 45 | list(APPEND ${VAR} "${lib}") 46 | endif() 47 | math(EXPR _index "${_index} + 1") 48 | endwhile() 49 | endmacro() 50 | 51 | # unpack a list of libraries with optional build configuration keyword prefixes 52 | # copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig 53 | # self contained 54 | macro(_unpack_libraries_with_build_configuration VAR) 55 | set(${VAR} "") 56 | foreach(lib ${ARGN}) 57 | string(REGEX REPLACE "^(debug|optimized|general)${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}(.+)$" "\\1;\\2" lib "${lib}") 58 | list(APPEND ${VAR} "${lib}") 59 | endforeach() 60 | endmacro() 61 | 62 | 63 | if(lilee_gazebo_CONFIG_INCLUDED) 64 | return() 65 | endif() 66 | set(lilee_gazebo_CONFIG_INCLUDED TRUE) 67 | 68 | # set variables for source/devel/install prefixes 69 | if("FALSE" STREQUAL "TRUE") 70 | set(lilee_gazebo_SOURCE_PREFIX /tmp/buildd/ros-hydro-turtlebot-gazebo-2.1.1-1raring-20140622-1227) 71 | set(lilee_gazebo_DEVEL_PREFIX /tmp/buildd/ros-hydro-turtlebot-gazebo-2.1.1-1raring-20140622-1227/obj-x86_64-linux-gnu/devel) 72 | set(lilee_gazebo_INSTALL_PREFIX "") 73 | set(lilee_gazebo_PREFIX ${lilee_gazebo_DEVEL_PREFIX}) 74 | else() 75 | set(lilee_gazebo_SOURCE_PREFIX "") 76 | set(lilee_gazebo_DEVEL_PREFIX "") 77 | set(lilee_gazebo_INSTALL_PREFIX /opt/ros/hydro) 78 | set(lilee_gazebo_PREFIX ${lilee_gazebo_INSTALL_PREFIX}) 79 | endif() 80 | 81 | # warn when using a deprecated package 82 | if(NOT "" STREQUAL "") 83 | set(_msg "WARNING: package 'lilee_gazebo' is deprecated") 84 | # append custom deprecation text if available 85 | if(NOT "" STREQUAL "TRUE") 86 | set(_msg "${_msg} ()") 87 | endif() 88 | message("${_msg}") 89 | endif() 90 | 91 | # flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project 92 | set(lilee_gazebo_FOUND_CATKIN_PROJECT TRUE) 93 | 94 | if(NOT "" STREQUAL "") 95 | set(lilee_gazebo_INCLUDE_DIRS "") 96 | set(_include_dirs "") 97 | foreach(idir ${_include_dirs}) 98 | if(IS_ABSOLUTE ${idir} AND IS_DIRECTORY ${idir}) 99 | set(include ${idir}) 100 | elseif("${idir}" STREQUAL "include") 101 | get_filename_component(include "${lilee_gazebo_DIR}/../../../include" ABSOLUTE) 102 | if(NOT IS_DIRECTORY ${include}) 103 | message(FATAL_ERROR "Project 'lilee_gazebo' specifies '${idir}' as an include dir, which is not found. It does not exist in '${include}'. Ask the maintainer 'Marcus Liebhardt ' to fix it.") 104 | endif() 105 | else() 106 | message(FATAL_ERROR "Project 'lilee_gazebo' specifies '${idir}' as an include dir, which is not found. It does neither exist as an absolute directory nor in '/opt/ros/hydro/${idir}'. Ask the maintainer 'Marcus Liebhardt ' to fix it.") 107 | endif() 108 | _list_append_unique(lilee_gazebo_INCLUDE_DIRS ${include}) 109 | endforeach() 110 | endif() 111 | 112 | set(libraries "") 113 | foreach(library ${libraries}) 114 | # keep build configuration keywords, target names and absolute libraries as-is 115 | if("${library}" MATCHES "^debug|optimized|general$") 116 | list(APPEND lilee_gazebo_LIBRARIES ${library}) 117 | elseif(TARGET ${library}) 118 | list(APPEND lilee_gazebo_LIBRARIES ${library}) 119 | elseif(IS_ABSOLUTE ${library}) 120 | list(APPEND lilee_gazebo_LIBRARIES ${library}) 121 | else() 122 | set(lib_path "") 123 | set(lib "${library}-NOTFOUND") 124 | # since the path where the library is found is returned we have to iterate over the paths manually 125 | foreach(path /opt/ros/hydro/lib;/opt/ros/hydro/lib) 126 | find_library(lib ${library} 127 | PATHS ${path} 128 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 129 | if(lib) 130 | set(lib_path ${path}) 131 | break() 132 | endif() 133 | endforeach() 134 | if(lib) 135 | _list_append_unique(lilee_gazebo_LIBRARY_DIRS ${lib_path}) 136 | list(APPEND lilee_gazebo_LIBRARIES ${lib}) 137 | else() 138 | # as a fall back for non-catkin libraries try to search globally 139 | find_library(lib ${library}) 140 | if(NOT lib) 141 | message(FATAL_ERROR "Project '${PROJECT_NAME}' tried to find library '${library}'. The library is neither a target nor built/installed properly. Did you compile project 'lilee_gazebo'? Did you find_package() it before the subdirectory containing its code is included?") 142 | endif() 143 | list(APPEND lilee_gazebo_LIBRARIES ${lib}) 144 | endif() 145 | endif() 146 | endforeach() 147 | 148 | set(lilee_gazebo_EXPORTED_TARGETS "") 149 | # create dummy targets for exported code generation targets to make life of users easier 150 | foreach(t ${lilee_gazebo_EXPORTED_TARGETS}) 151 | if(NOT TARGET ${t}) 152 | add_custom_target(${t}) 153 | endif() 154 | endforeach() 155 | 156 | set(depends "") 157 | foreach(depend ${depends}) 158 | string(REPLACE " " ";" depend_list ${depend}) 159 | # the package name of the dependency must be kept in a unique variable so that it is not overwritten in recursive calls 160 | list(GET depend_list 0 lilee_gazebo_dep) 161 | list(LENGTH depend_list count) 162 | if(${count} EQUAL 1) 163 | # simple dependencies must only be find_package()-ed once 164 | if(NOT ${lilee_gazebo_dep}_FOUND) 165 | find_package(${lilee_gazebo_dep} REQUIRED) 166 | endif() 167 | else() 168 | # dependencies with components must be find_package()-ed again 169 | list(REMOVE_AT depend_list 0) 170 | find_package(${lilee_gazebo_dep} REQUIRED ${depend_list}) 171 | endif() 172 | _list_append_unique(lilee_gazebo_INCLUDE_DIRS ${${lilee_gazebo_dep}_INCLUDE_DIRS}) 173 | 174 | # merge build configuration keywords with library names to correctly deduplicate 175 | _pack_libraries_with_build_configuration(lilee_gazebo_LIBRARIES ${lilee_gazebo_LIBRARIES}) 176 | _pack_libraries_with_build_configuration(_libraries ${${lilee_gazebo_dep}_LIBRARIES}) 177 | _list_append_deduplicate(lilee_gazebo_LIBRARIES ${_libraries}) 178 | # undo build configuration keyword merging after deduplication 179 | _unpack_libraries_with_build_configuration(lilee_gazebo_LIBRARIES ${lilee_gazebo_LIBRARIES}) 180 | 181 | _list_append_unique(lilee_gazebo_LIBRARY_DIRS ${${lilee_gazebo_dep}_LIBRARY_DIRS}) 182 | list(APPEND lilee_gazebo_EXPORTED_TARGETS ${${lilee_gazebo_dep}_EXPORTED_TARGETS}) 183 | endforeach() 184 | 185 | set(pkg_cfg_extras "") 186 | foreach(extra ${pkg_cfg_extras}) 187 | if(NOT IS_ABSOLUTE ${extra}) 188 | set(extra ${lilee_gazebo_DIR}/${extra}) 189 | endif() 190 | include(${extra}) 191 | endforeach() 192 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/edit_maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/edit_maze_pg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/edit_maze_qlearning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/includes/create.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/includes/kobuki.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/includes/roomba.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/launch_maze.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/launch_maze_pg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /lilee_gazebo/launch/launch_maze_qlearning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /lilee_gazebo/maps/playground.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RLAgent/Robotics-RL/8f509f38ed94c005561c14ed8f60b65066d9e3c9/lilee_gazebo/maps/playground.pgm -------------------------------------------------------------------------------- /lilee_gazebo/maps/playground.yaml: -------------------------------------------------------------------------------- 1 | free_thresh: 0.196 2 | image: playground.pgm 3 | negate: 0 4 | occupied_thresh: 0.65 5 | origin: [-6.8999999999999915, -5.8999999999999915, 0.0] 6 | resolution: 0.05 7 | -------------------------------------------------------------------------------- /lilee_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | lilee_gazebo 3 | 2.1.1 4 | Gazebo launchers and worlds for TurtleBot simulation 5 | Marcus Liebhardt 6 | BSD 7 | http://ros.org/wiki/lilee_gazebo 8 | https://github.com/turtlebot/turtlebot_simulator 9 | https://github.com/turtlebot/turtlebot_simulator/issues 10 | Willow Garage 11 | 12 | catkin 13 | 14 | yocs_cmd_vel_mux 15 | create_gazebo_plugins 16 | diagnostic_aggregator 17 | depthimage_to_laserscan 18 | gazebo_ros 19 | kobuki_gazebo_plugins 20 | robot_pose_ekf 21 | robot_state_publisher 22 | turtlebot_bringup 23 | turtlebot_description 24 | xacro 25 | 26 | -------------------------------------------------------------------------------- /lilee_gazebo/src/environment.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include // rand 3 | #include // std::abs 4 | #include "gazebo_msgs/GetModelState.h" 5 | #include "gazebo_msgs/ModelState.h" 6 | #include "std_msgs/Float32.h" 7 | #include "std_msgs/String.h" 8 | #include "geometry_msgs/Pose.h" 9 | 10 | std::string goal_model_name = "fire_hydrant"; 11 | std::string agent_model_name = "mobile_base"; 12 | 13 | ros::ServiceClient getmodelstate; 14 | ros::Publisher respawner_pub, reward_pub; 15 | gazebo_msgs::GetModelState agent_getmodelstate, goal_getmodelstate; 16 | 17 | int counter; 18 | 19 | // Respawn goal model randomly on the map. 20 | void respawn() 21 | { 22 | gazebo_msgs::ModelState goal_modelstate; 23 | 24 | goal_modelstate.model_name = goal_model_name; 25 | goal_modelstate.reference_frame = "world"; 26 | 27 | geometry_msgs::Pose new_pose; 28 | new_pose.position.x = rand() % 20 - 10; new_pose.position.y = rand() % 20 - 10; new_pose.position.z = 0; 29 | new_pose. orientation.w = 1.0; new_pose.orientation.x = new_pose.orientation.y = new_pose.orientation.z = 0; 30 | goal_modelstate.pose = new_pose; 31 | 32 | respawner_pub.publish(goal_modelstate); 33 | ROS_INFO("Respawned goal to (%f, %f, %f).", new_pose.position.x, new_pose.position.y, new_pose.position.z); 34 | } 35 | 36 | void send_reward(float32 val) 37 | { 38 | std_msgs::Float32 reward; 39 | reward.data = val; 40 | reward_pub.publish(reward); 41 | 42 | ROS_INFO("environment: Sent reward %f", val); 43 | } 44 | 45 | // Turtlebot has reached goal state. Publish reward and respawn goal model. 46 | void goal_state() 47 | { 48 | ROS_INFO("In goal_state()"); 49 | respawn(); 50 | send_reward(1000.0); 51 | } 52 | 53 | int main(int argc, char **argv) 54 | { 55 | ros::init(argc, argv, "environment"); 56 | ros::NodeHandle n; 57 | srand (time(NULL)); 58 | 59 | getmodelstate = n.serviceClient("/gazebo/get_model_state"); 60 | reward_pub = n.advertise("rewards", 1000); 61 | respawner_pub = n.advertise("/gazebo/set_model_state", 1); 62 | 63 | agent_getmodelstate.request.model_name = agent_model_name; 64 | goal_getmodelstate.request.model_name = goal_model_name; 65 | 66 | while (ros::ok()) 67 | { 68 | { 69 | if (getmodelstate.call(agent_getmodelstate) && getmodelstate.call(goal_getmodelstate)) 70 | { 71 | if ( 72 | (std::abs(agent_getmodelstate.response.pose.position.x - goal_getmodelstate.response.pose.position.x) < 0.8) && 73 | (std::abs(agent_getmodelstate.response.pose.position.y - goal_getmodelstate.response.pose.position.y) < 0.8) && 74 | (std::abs(agent_getmodelstate.response.pose.position.z - goal_getmodelstate.response.pose.position.z) < 0.1) 75 | ) 76 | { 77 | //goal_state(); 78 | ros::spinOnce(); 79 | ros::Duration(5).sleep(); 80 | } 81 | else 82 | { 83 | // publish -1 for step cost 84 | } 85 | } 86 | } 87 | } 88 | 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /lilee_gazebo/src/world_control.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include //std::string 3 | 4 | #include "lilee_rl_continuous/world_control.h" 5 | #include "gazebo_msgs/SpawnModel.h" 6 | #include "gazebo_msgs/DeleteModel.h" 7 | #include "std_srvs/Empty.h" 8 | #include "lilee_rl_continuous/WorldControl.h" 9 | 10 | namespace world_control 11 | { 12 | int NUM_WORLDS_ = 5; 13 | } 14 | 15 | ros::ServiceClient pause_client, unpause_client, gazebo_spawn_client, gazebo_delete_client; 16 | ros::ServiceServer worldcontrolsrv_; 17 | 18 | /********************************************************* 19 | * Pause/Unpause world 20 | *********************************************************/ 21 | int pauseWorld() 22 | { 23 | std_srvs::Empty empty; 24 | if (!pause_client.call(empty)) 25 | { 26 | ROS_ERROR("unable to pause physics engine"); 27 | return -1; 28 | } 29 | return 0; 30 | } 31 | 32 | int unpauseWorld() 33 | { 34 | std_srvs::Empty empty; 35 | if (!unpause_client.call(empty)) 36 | { 37 | ROS_ERROR("unable to unpause physics engine"); 38 | return -1; 39 | } 40 | return 0; 41 | } 42 | 43 | /********************************************************* 44 | * Change friction of 'ground_plane' model to (mu, mu2) 45 | *********************************************************/ 46 | int changeFriction(double mu, double mu2) 47 | { 48 | pauseWorld(); 49 | 50 | char buffer [780]; 51 | sprintf(buffer, 52 | "10 0 1100 100%f%f1000 0 1100 10000001", 53 | mu, mu2 54 | ); 55 | 56 | // Delete 'ground_plane' 57 | gazebo_msgs::DeleteModel deletemodel; 58 | deletemodel.request.model_name = "ground_plane"; 59 | gazebo_delete_client.call(deletemodel); 60 | 61 | // respawn new 'ground_plane' 62 | gazebo_msgs::SpawnModel model; 63 | model.request.model_xml = buffer; 64 | 65 | 66 | model.request.model_name="ground_plane"; 67 | model.request.reference_frame="world"; 68 | if (!gazebo_spawn_client.call(model)) 69 | { 70 | ROS_ERROR("world_control: Failed to respawn new 'ground_plane'"); 71 | return -1; 72 | } 73 | else 74 | ROS_ERROR("world_control: Changed friction to (%f, %f)", mu, mu2); 75 | 76 | unpauseWorld(); 77 | 78 | return 0; 79 | } 80 | 81 | int changeWorld(int numWorld) 82 | { // Change world to numWorld 83 | 84 | if (numWorld > world_control::NUM_WORLDS_) 85 | { 86 | ROS_ERROR("world_control: Invalid world number"); 87 | return -1; 88 | } 89 | 90 | if (numWorld == 1) 91 | changeFriction(100, 50); 92 | else if (numWorld == 2) 93 | changeFriction(5,10); 94 | else if (numWorld == 3) 95 | changeFriction(100, 1.5); 96 | else if (numWorld == 4) 97 | changeFriction(3, 1.5); 98 | else if (numWorld == 5) 99 | changeFriction(0.6, 50); 100 | 101 | return 0; 102 | } 103 | /********************************************************* 104 | * Callback 105 | *********************************************************/ 106 | bool worldcontrolcb(lilee_rl_continuous::WorldControl::Request& req, lilee_rl_continuous::WorldControl::Response& res) 107 | { 108 | int iSuccess = changeWorld((int)req.numWorld); 109 | if (iSuccess != 0) 110 | return false; 111 | else 112 | return true; 113 | } 114 | 115 | /********************************************************* 116 | * Main 117 | *********************************************************/ 118 | int main(int argc, char **argv) 119 | { 120 | ros::init(argc, argv, "world_control"); 121 | ros::NodeHandle n; 122 | 123 | pause_client = n.serviceClient("/gazebo/pause_physics"); 124 | pause_client.waitForExistence(); 125 | 126 | unpause_client = n.serviceClient("/gazebo/unpause_physics"); 127 | unpause_client.waitForExistence(); 128 | 129 | gazebo_spawn_client = n.serviceClient ("/gazebo/spawn_sdf_model"); 130 | gazebo_spawn_client.waitForExistence(); 131 | 132 | gazebo_delete_client = n.serviceClient ("/gazebo/delete_model"); 133 | gazebo_delete_client.waitForExistence(); 134 | 135 | worldcontrolsrv_ = n.advertiseService("/world_control", worldcontrolcb); 136 | 137 | ros::spin(); 138 | 139 | return 0; 140 | } 141 | -------------------------------------------------------------------------------- /lilee_gazebo/terminal_cmds: -------------------------------------------------------------------------------- 1 | rostopic pub -r 20 /gazebo/set_model_state gazebo_msgs/ModelState '{model_name: fire_hydrant, pose: { position: { x: 0, y: 0, z: 0 }, orientation: {x: 0, y: 0, z: 0, w: 1 } }, twist: { linear: { x: 0, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0} }, reference_frame: world }' 2 | 3 | 4 | rosservice call /gazebo/get_model_state '{model_name: fire_hydrant}' 5 | rosservice call /gazebo/get_model_state '{model_name: mobile_base}' 6 | 7 | 8 | -------------------------------------------------------------------------------- /lilee_gazebo/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | 0.01 15 | 1 16 | 100 17 | 0 0 -9.8 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /lilee_gazebo/worlds/lilee.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 10 26 | 27 | 28 | 0 29 | 30 | 31 | 0 0 1 32 | 100 100 33 | 34 | 35 | 36 | 40 | 41 | 42 | 43 | 0 44 | 0 45 | 46 | 0 47 | 0 48 | 1 49 | 50 | 51 | 52 | 1 53 | 0 0 10 0 -0 0 54 | 0.8 0.8 0.8 1 55 | 0.2 0.2 0.2 1 56 | 57 | 1000 58 | 0.9 59 | 0.01 60 | 0.001 61 | 62 | -0.5 0.1 -0.9 63 | 64 | 65 | 1 66 | 67 | 0 0 0 0 -0 0 68 | 69 | 70 | 71 | model://gas_station/meshes/gas_station.dae 72 | 73 | 74 | 10 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | model://gas_station/meshes/gas_station.dae 89 | 90 | 91 | 92 | 97 | 98 | GasStation_Normal.png 99 | 100 | 101 | 102 | 103 | 0 104 | 0 105 | 106 | 0 107 | 0 108 | 1 109 | 110 | -2 7 0 0 -0 0 111 | 112 | 113 | 0.001 114 | 1 115 | 1000 116 | 0 0 -9.8 117 | 118 | 119 | 0.4 0.4 0.4 1 120 | 0.7 0.7 0.7 1 121 | 1 122 | 123 | 124 | 125 | 0 0 0.5 0 -0 0 126 | 127 | 128 | 129 | model://cube_20k/meshes/cube_20k.stl 130 | 0.5 0.5 0.5 131 | 132 | 133 | 10 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | model://cube_20k/meshes/cube_20k.stl 148 | 0.5 0.5 0.5 149 | 150 | 151 | 152 | 153 | 0 154 | 0 155 | 156 | 0 157 | 158 | 159 | 1 160 | 0 161 | 0 162 | 1 163 | 0 164 | 1 165 | 166 | 1 167 | 168 | 0 169 | 1 170 | 171 | 21 -5 0 0 -0 0 172 | 0 173 | 174 | 175 | 201 893000000 176 | 203 324723420 177 | 1404757036 248314873 178 | 179 | 21 -5 -0 -0 -0 -2e-06 180 | 181 | 21 -5 0.5 -0 -0 -2e-06 182 | 0 0 0 0 -0 0 183 | 0 0 0 0 -0 0 184 | 0 0 0 0 -0 0 185 | 186 | 187 | 188 | 2.51441 6.51913 0 0 -0 0 189 | 190 | 2.51441 6.51913 0 0 -0 0 191 | 0 0 0 0 -0 0 192 | 0 0 0 0 -0 0 193 | 0 0 0 0 -0 0 194 | 195 | 196 | 197 | 0 0 0 0 -0 0 198 | 199 | 0 0 0 0 -0 0 200 | 0 0 0 0 -0 0 201 | 0 0 0 0 -0 0 202 | 0 0 0 0 -0 0 203 | 204 | 205 | 206 | 207 | 208 | 17.3004 -67.5869 38.2184 0 0.399643 1.6802 209 | orbit 210 | 211 | 212 | 213 | 214 | -------------------------------------------------------------------------------- /lilee_gazebo/worlds/maze_pg.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 10 39 | 40 | 41 | 0 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 0 60 | 0 61 | 1 62 | 63 | 64 | 65 | 0.001 66 | 1 67 | 1000 68 | 0 0 -9.8 69 | 70 | 71 | 0.4 0.4 0.4 1 72 | 0.7 0.7 0.7 1 73 | 1 74 | 75 | 76 | 1547 91000000 77 | 92 112940725 78 | 1406916118 710575495 79 | 80 | 1.98503 1.92642 0.002102 -1e-06 -9e-06 1e-06 81 | 82 | 1.98503 1.92642 0.002102 -1e-06 -9e-06 1e-06 83 | 0 0 0 0 -0 0 84 | 0 0 0 0 -0 0 85 | 0 0 0 0 -0 0 86 | 87 | 88 | 89 | 0 0 0 0 -0 0 90 | 91 | 0 0 0 0 -0 0 92 | 0 0 0 0 -0 0 93 | 0 0 0 0 -0 0 94 | 0 0 0 0 -0 0 95 | 96 | 97 | 98 | 0.0347 10.057 1 0 -0 0 99 | 100 | 0.0347 10.057 1 0 -0 0 101 | 0 0 0 0 -0 0 102 | 0 0 0 0 -0 0 103 | 0 0 0 0 -0 0 104 | 105 | 106 | 107 | 0.009735 -10.1368 1 0 -0 0 108 | 109 | 0.009735 -10.1368 1 0 -0 0 110 | 0 0 0 0 -0 0 111 | 0 0 0 0 -0 0 112 | 0 0 0 0 -0 0 113 | 114 | 115 | 116 | 10.1418 -0.005623 1 0 -0 0 117 | 118 | 10.1418 -0.005623 1 0 -0 0 119 | 0 0 0 0 -0 0 120 | 0 0 0 0 -0 0 121 | 0 0 0 0 -0 0 122 | 123 | 124 | 125 | -10.0605 -0.008208 1 0 -0 0 126 | 127 | -10.0605 -0.008208 1 0 -0 0 128 | 0 0 0 0 -0 0 129 | 0 0 0 0 -0 0 130 | 0 0 0 0 -0 0 131 | 132 | 133 | 134 | 135 | 136 | 2.76507 -0.252041 32.8956 9e-06 1.5698 1.58024 137 | orbit 138 | 139 | 140 | 141 | 0 142 | 143 | 144 | 145 | 146 | model://fire_hydrant/meshes/fire_hydrant.dae 147 | 148 | 149 | 10 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | model://fire_hydrant/meshes/fire_hydrant.dae 164 | 165 | 166 | 167 | 168 | 0 169 | 0 170 | 171 | 0 172 | 0 173 | 1 174 | 175 | 176 | 1 177 | 0 178 | 0 179 | 1 180 | 0 181 | 1 182 | 183 | 1 184 | 185 | 186 | 2 -2 0 0 -0 0 187 | 188 | 189 | 0 10 1 0 -0 0 190 | 191 | 192 | 193 | 194 | 20 0.2 2 195 | 196 | 197 | 10 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 20 0.2 2 212 | 213 | 214 | 215 | 219 | 220 | 221 | 222 | 0 223 | 0 224 | 225 | 0 226 | 0 227 | 1 228 | 229 | 1 230 | 231 | 232 | 0 -10 1 0 -0 0 233 | 234 | 235 | 236 | 237 | 20 0.2 2 238 | 239 | 240 | 10 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 20 0.2 2 255 | 256 | 257 | 258 | 262 | 263 | 264 | 265 | 0 266 | 0 267 | 268 | 0 269 | 0 270 | 1 271 | 272 | 1 273 | 274 | 275 | 10 0 1 0 -0 0 276 | 277 | 278 | 279 | 280 | 0.2 20 2 281 | 282 | 283 | 10 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 0.2 20 2 298 | 299 | 300 | 301 | 305 | 306 | 307 | 308 | 0 309 | 0 310 | 311 | 0 312 | 0 313 | 1 314 | 315 | 1 316 | 317 | 318 | -10 0 1 0 -0 0 319 | 320 | 321 | 322 | 323 | 0.2 20 2 324 | 325 | 326 | 10 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 0.2 20 2 341 | 342 | 343 | 344 | 348 | 349 | 350 | 351 | 0 352 | 0 353 | 354 | 0 355 | 0 356 | 1 357 | 358 | 1 359 | 360 | 361 | 362 | -------------------------------------------------------------------------------- /lilee_gazebo/worlds/maze_qlearning.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 29 | 100 30 | 50 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 10 39 | 40 | 41 | 0 42 | 43 | 44 | 0 0 1 45 | 100 100 46 | 47 | 48 | 49 | 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 0 60 | 0 61 | 1 62 | 63 | 64 | 65 | 0.001 66 | 1 67 | 1000 68 | 0 0 -9.8 69 | 70 | 71 | 0.4 0.4 0.4 1 72 | 0.7 0.7 0.7 1 73 | 1 74 | 75 | 76 | 1513 235000000 77 | 57 996840660 78 | 1407006919 466640896 79 | 80 | 1.98503 1.92642 0.002102 -3e-06 -5e-06 -3.23117e-27 81 | 82 | 1.98503 1.92642 0.002102 -3e-06 -5e-06 -3.23117e-27 83 | 0 0 0 0 -0 0 84 | 0 0 0 0 -0 0 85 | 0 0 0 0 -0 0 86 | 87 | 88 | 89 | 0 0 0 0 -0 0 90 | 91 | 0 0 0 0 -0 0 92 | 0 0 0 0 -0 0 93 | 0 0 0 0 -0 0 94 | 0 0 0 0 -0 0 95 | 96 | 97 | 98 | 0.030413 9.89028 1 0 -0 0 99 | 100 | 0.030413 9.89028 1 0 -0 0 101 | 0 0 0 0 -0 0 102 | 0 0 0 0 -0 0 103 | 0 0 0 0 -0 0 104 | 105 | 106 | 107 | 0.042642 -10.1135 1 0 -0 0 108 | 109 | 0.042642 -10.1135 1 0 -0 0 110 | 0 0 0 0 -0 0 111 | 0 0 0 0 -0 0 112 | 0 0 0 0 -0 0 113 | 114 | 115 | 116 | 10.0321 -0.079508 1 0 -0 0 117 | 118 | 10.0321 -0.079508 1 0 -0 0 119 | 0 0 0 0 -0 0 120 | 0 0 0 0 -0 0 121 | 0 0 0 0 -0 0 122 | 123 | 124 | 125 | -10.0294 -0.019264 1 0 -0 0 126 | 127 | -10.0294 -0.019264 1 0 -0 0 128 | 0 0 0 0 -0 0 129 | 0 0 0 0 -0 0 130 | 0 0 0 0 -0 0 131 | 132 | 133 | 134 | 135 | 136 | 2.40262 -0.469932 34.5871 9e-06 1.5698 1.58024 137 | orbit 138 | 139 | 140 | 141 | 0 142 | 143 | 144 | 145 | 146 | model://fire_hydrant/meshes/fire_hydrant.dae 147 | 148 | 149 | 10 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | model://fire_hydrant/meshes/fire_hydrant.dae 164 | 165 | 166 | 167 | 168 | 0 169 | 0 170 | 171 | 0 172 | 0 173 | 1 174 | 175 | 176 | 1 177 | 0 178 | 0 179 | 1 180 | 0 181 | 1 182 | 183 | 1 184 | 185 | 186 | 2 -2 0 0 -0 0 187 | 188 | 189 | 0 10 1 0 -0 0 190 | 191 | 192 | 193 | 194 | 20 0.2 2 195 | 196 | 197 | 10 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 20 0.2 2 212 | 213 | 214 | 215 | 219 | 220 | 221 | 222 | 0 223 | 0 224 | 225 | 0 226 | 0 227 | 1 228 | 229 | 1 230 | 231 | 232 | 0 -10 1 0 -0 0 233 | 234 | 235 | 236 | 237 | 20 0.2 2 238 | 239 | 240 | 10 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 20 0.2 2 255 | 256 | 257 | 258 | 262 | 263 | 264 | 265 | 0 266 | 0 267 | 268 | 0 269 | 0 270 | 1 271 | 272 | 1 273 | 274 | 275 | 10 0 1 0 -0 0 276 | 277 | 278 | 279 | 280 | 0.2 20 2 281 | 282 | 283 | 10 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 0.2 20 2 298 | 299 | 300 | 301 | 305 | 306 | 307 | 308 | 0 309 | 0 310 | 311 | 0 312 | 0 313 | 1 314 | 315 | 1 316 | 317 | 318 | -10 0 1 0 -0 0 319 | 320 | 321 | 322 | 323 | 0.2 20 2 324 | 325 | 326 | 10 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 0.2 20 2 341 | 342 | 343 | 344 | 348 | 349 | 350 | 351 | 0 352 | 0 353 | 354 | 0 355 | 0 356 | 1 357 | 358 | 1 359 | 360 | 361 | 362 | -------------------------------------------------------------------------------- /lilee_rl_continuous/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lilee_rl_continuous) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | message_generation 9 | roscpp 10 | rospy 11 | std_msgs 12 | nodelet 13 | pcl_ros 14 | turtlebot_msgs 15 | kobuki_msgs 16 | geometry_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | find_package(Boost REQUIRED) 21 | find_package(PCL) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 40 | ## pulled in transitively but can be declared for certainty nonetheless: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | GoalLocation.msg 57 | State.msg 58 | AgentLocation.msg 59 | TeleopCmd.msg 60 | ContinuousAction.msg 61 | ) 62 | 63 | ## Generate services in the 'srv' folder 64 | add_service_files( 65 | FILES 66 | AgentEnvironment.srv 67 | WorldControl.srv 68 | ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | kobuki_msgs 82 | ) 83 | 84 | ################################### 85 | ## catkin specific configuration ## 86 | ################################### 87 | ## The catkin_package macro generates cmake config files for your package 88 | ## Declare things to be passed to dependent projects 89 | ## INCLUDE_DIRS: uncomment this if you package contains header files 90 | ## LIBRARIES: libraries you create in this project that dependent projects also need 91 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 92 | ## DEPENDS: system dependencies of this project that dependent projects also need 93 | catkin_package( 94 | INCLUDE_DIRS 95 | LIBRARIES ${PROJECT_NAME} turtlebot_teleop 96 | CATKIN_DEPENDS nodelet pcl_ros roscpp turtlebot_msgs kobuki_msgs geometry_msgs 97 | DEPENDS Boost PCL 98 | ) 99 | 100 | ########### 101 | ## Build ## 102 | ########### 103 | 104 | ## Specify additional locations of header files 105 | ## Your package locations should be listed before other locations 106 | # include_directories(include) 107 | include_directories( 108 | include 109 | ${catkin_INCLUDE_DIRS} 110 | ${PCL_INCLUDE_DIRS} 111 | ${Boost_INCLUDE_DIRS} 112 | ) 113 | 114 | ## Declare a cpp library 115 | add_library(${PROJECT_NAME} src/environment.cpp src/actions.cpp) 116 | 117 | add_dependencies(${PROJECT_NAME} turtlebot_msgs_gencpp) 118 | add_dependencies(${PROJECT_NAME} kobuki_msgs_gencpp) 119 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 120 | add_dependencies(${PROJECT_NAME} lilee_rl_continuous_generate_messages_cpp) 121 | #add_dependencies(${PROJECT_NAME} lilee_rl_continuous_gencpp) 122 | 123 | target_link_libraries(${PROJECT_NAME} 124 | ${catkin_LIBRARIES} 125 | ${PCL_LIBRARIES} 126 | ) 127 | 128 | add_executable(agent_pg src/agent_pg.cpp) 129 | target_link_libraries(agent_pg ${PROJECT_NAME} ${catkin_LIBRARIES}) 130 | add_dependencies(agent_pg lilee_rl_continuous_generate_messages_cpp) 131 | 132 | add_executable(agent_pg-ella src/agent_pg-ella.cpp) 133 | target_link_libraries(agent_pg-ella ${PROJECT_NAME} ${catkin_LIBRARIES}) 134 | add_dependencies(agent_pg-ella lilee_rl_continuous_generate_messages_cpp) 135 | 136 | add_executable(continuousTeleop src/teleop.cpp src/actions.cpp) 137 | target_link_libraries(continuousTeleop ${catkin_LIBRARIES}) 138 | add_dependencies(continuousTeleop lilee_rl_continuous_generate_messages_cpp) 139 | 140 | add_executable(world_control src/world_control.cpp) 141 | target_link_libraries(world_control ${PROJECT_NAME} ${catkin_LIBRARIES}) 142 | add_dependencies(world_control lilee_rl_continuous_generate_messages_cpp) 143 | 144 | ## Declare a cpp executable 145 | # add_executable(lilee_agent_node src/lilee_agent_node.cpp) 146 | 147 | ## Add cmake target dependencies of the executable/library 148 | ## as an example, message headers may need to be generated before nodes 149 | # add_dependencies(lilee_agent_node lilee_agent_generate_messages_cpp) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | 170 | install(TARGETS ${PROJECT_NAME} 171 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | ) 175 | 176 | install(TARGETS lilee_rl_continuous 177 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 180 | ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | install(DIRECTORY sim 191 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | ) 193 | 194 | install(DIRECTORY launch 195 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | ) 197 | 198 | install(DIRECTORY param 199 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 200 | ) 201 | 202 | install(DIRECTORY plugins 203 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 204 | ) 205 | 206 | ############# 207 | ## Testing ## 208 | ############# 209 | 210 | ## Add gtest based cpp test target and link libraries 211 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lilee_agent.cpp) 212 | # if(TARGET ${PROJECT_NAME}-test) 213 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 214 | # endif() 215 | 216 | ## Add folders to be run by python nosetests 217 | # catkin_add_nosetests(test) 218 | 219 | 220 | 221 | 222 | -------------------------------------------------------------------------------- /lilee_rl_continuous/include/lilee_rl_continuous/actions.h: -------------------------------------------------------------------------------- 1 | #ifndef ACTIONS_H_ 2 | #define ACTIONS_H_ 3 | #include "geometry_msgs/Twist.h" 4 | 5 | namespace actions 6 | { 7 | extern int NUM_ACTIONS_; 8 | extern double ACTION_DURATION_; 9 | 10 | enum Move 11 | { 12 | FORWARD_ = 0, 13 | TURN_LEFT_ = 1, 14 | TURN_RIGHT_ = 2, 15 | STOP_ = 3 16 | }; 17 | 18 | geometry_msgs::TwistPtr move_forward(); 19 | geometry_msgs::TwistPtr move_turnleft(); 20 | geometry_msgs::TwistPtr move_turnright(); 21 | geometry_msgs::TwistPtr stop(); 22 | geometry_msgs::TwistPtr executeAction(int action); 23 | 24 | } 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /lilee_rl_continuous/include/lilee_rl_continuous/gazebo_parameters.h: -------------------------------------------------------------------------------- 1 | #ifndef GAZEBO_PARAMETERS_H_ 2 | #define GAZEBO_PARAMETERS_H_ 3 | 4 | int WORLD_BOUND_ = 8; // size of world 5 | 6 | int AGENT_INIT_POS_X = 1; 7 | int AGENT_INIT_POS_Y = 2; 8 | int AGENT_INIT_POS_Z = 0; 9 | 10 | std::string goal_model_name = "fire_hydrant"; 11 | std::string agent_model_name = "mobile_base"; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /lilee_rl_continuous/include/lilee_rl_continuous/world_control.h: -------------------------------------------------------------------------------- 1 | #ifndef WORLD_CONTROL_H_ 2 | #define WORLD_CONTROL_H_ 3 | 4 | namespace world_control 5 | { 6 | extern int NUM_WORLDS_; 7 | } 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /lilee_rl_continuous/launch/includes/velocity_smoother.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /lilee_rl_continuous/launch/includes/velocity_smoother.launch.xml~: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /lilee_rl_continuous/launch/pg-ella.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /lilee_rl_continuous/launch/pg.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /lilee_rl_continuous/msg/AgentLocation.msg: -------------------------------------------------------------------------------- 1 | float64 position_x 2 | float64 position_y 3 | float64 orientation_yaw 4 | -------------------------------------------------------------------------------- /lilee_rl_continuous/msg/ContinuousAction.msg: -------------------------------------------------------------------------------- 1 | float32 linear_vel 2 | float32 angular_vel 3 | -------------------------------------------------------------------------------- /lilee_rl_continuous/msg/GoalLocation.msg: -------------------------------------------------------------------------------- 1 | float32 position_x 2 | float32 position_z 3 | -------------------------------------------------------------------------------- /lilee_rl_continuous/msg/State.msg: -------------------------------------------------------------------------------- 1 | lilee_rl_continuous/GoalLocation goal 2 | lilee_rl_continuous/AgentLocation agent_location 3 | kobuki_msgs/BumperEvent bumper 4 | float32 distance_to_goal 5 | float32 theta_to_goal 6 | bool isTerminal 7 | bool success 8 | -------------------------------------------------------------------------------- /lilee_rl_continuous/msg/TeleopCmd.msg: -------------------------------------------------------------------------------- 1 | bool user_control 2 | lilee_rl_continuous/ContinuousAction action 3 | -------------------------------------------------------------------------------- /lilee_rl_continuous/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lilee_rl_continuous 4 | 0.0.0 5 | The lilee_rl_continuous package 6 | 7 | 8 | 9 | 10 | Lisa Lee 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | nodelet 45 | pcl_ros 46 | message_generation 47 | roscpp 48 | rospy 49 | std_msgs 50 | turtlebot_msgs 51 | kobuki_msgs 52 | cv_bridge 53 | sensor_msgs 54 | image_transport 55 | geometry_msgs 56 | 57 | roscpp 58 | rospy 59 | std_msgs 60 | nodelet 61 | pcl_ros 62 | turtlebot_msgs 63 | kobuki_msgs 64 | cv_bridge 65 | sensor_msgs 66 | image_transport 67 | geometry_msgs 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /lilee_rl_continuous/param/mux.yaml: -------------------------------------------------------------------------------- 1 | # Created on: Dec 5, 2012 2 | # Author: jorge 3 | # Configuration for subscribers to cmd_vel sources. 4 | # 5 | # Individual subscriber configuration: 6 | # name: Source name 7 | # topic: The topic that provides cmd_vel messages 8 | # timeout: Time in seconds without incoming messages to consider this topic inactive 9 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 10 | # short_desc: Short description (optional) 11 | 12 | subscribers: 13 | - name: "Safe reactive controller" 14 | topic: "input/safety_controller" 15 | timeout: 0.2 16 | priority: 10 17 | - name: "Reinforcement" 18 | topic: "input/reinforcement" 19 | timeout: 0.5 20 | priority: 7 21 | -------------------------------------------------------------------------------- /lilee_rl_continuous/plugins/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The turtlebot people reinforcement node. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /lilee_rl_continuous/src/actions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "geometry_msgs/Twist.h" 3 | #include "lilee_rl_continuous/actions.h" // parameters for agent's actions 4 | 5 | namespace actions { 6 | double ACTION_DURATION_ = 0.5; // duration of each action (sec) 7 | double LINEAR_X_ = 0.6; // linear.x velocity 8 | double ANGULAR_Z = 0.45; 9 | } 10 | -------------------------------------------------------------------------------- /lilee_rl_continuous/src/environment.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include "std_msgs/String.h" 9 | #include 10 | #include 11 | 12 | #include // quiet_NaN() 13 | #include //floor 14 | #include // htonl 15 | #include // rand 16 | #include // std::abs 17 | #include 18 | 19 | #include "gazebo_msgs/GetModelState.h" // Agent's GPS location 20 | #include "gazebo_msgs/ModelState.h" // respawn 21 | #include "geometry_msgs/Pose.h" // respawn 22 | #include 23 | #include // bumper 24 | 25 | #include "lilee_rl_continuous/AgentEnvironment.h" 26 | #include "lilee_rl_continuous/State.h" 27 | #include "lilee_rl_continuous/actions.h" // parameters for agent's actions 28 | #include "lilee_rl_continuous/gazebo_parameters.h" // parameters for gazebo 29 | 30 | namespace lilee_rl_continuous 31 | { 32 | typedef pcl::PointCloud PointCloud; 33 | /** 34 | * Subscribes to point clouds from the 3dsensor, 35 | * processes them, and publishes state messages. 36 | */ 37 | 38 | class LileeEnvironment : public nodelet::Nodelet 39 | { 40 | public: 41 | /*! 42 | * @brief The constructor for the reinforcement. 43 | * Constructor for the reinforcement. 44 | */ 45 | LileeEnvironment() : min_y_(0.1), max_y_(0.5), 46 | min_x_(-0.2), max_x_(0.2), 47 | max_z_(0.8), goal_z_(0.6), 48 | z_scale_(1.0), x_scale_(5.0) 49 | { 50 | } 51 | 52 | ///private: 53 | double min_y_; /**< The minimum y position of the points in the box. */ 54 | double max_y_; /**< The maximum y position of the points in the box. */ 55 | double min_x_; /**< The minimum x position of the points in the box. */ 56 | double max_x_; /**< The maximum x position of the points in the box. */ 57 | double max_z_; /**< The maximum z position of the points in the box. */ 58 | double goal_z_; /**< The distance away from the robot to hold the centroid */ 59 | double z_scale_; /**< The scaling factor for translational robot speed */ 60 | double x_scale_; /**< The scaling factor for rotational robot speed */ 61 | bool enabled_; /**< Enable/disable following; just prevents motor commands */ 62 | int ul_x, ul_y, lr_x, lr_y; /** coordinate of rectangle vertices */ 63 | float focal_length_; /** focal length of camera */ 64 | bool receivedRectCoord; 65 | 66 | /** Communication with agent */ 67 | ros::ServiceServer reinforcement_srv_; 68 | bool goaldepthFlag, odomFlag; 69 | lilee_rl_continuous::State state_; 70 | float DIST_TO_GOAL_; 71 | lilee_rl_continuous::State TERMINAL_STATE_; 72 | int episodeNum, stepCnt, MAX_STEP_CNT_; 73 | 74 | int ANGLE_DIVIDER_, DIST_DIVIDER_; 75 | 76 | /** Agent's GPS location */ 77 | ros::ServiceClient getmodelstate_; 78 | 79 | 80 | 81 | /********************************************************* 82 | * @brief OnInit method from node handle. 83 | * OnInit method from node handle. Sets up the parameters 84 | * and topics. 85 | *********************************************************/ 86 | virtual void onInit() 87 | { 88 | ROS_ERROR("envir: hello from onInit()"); 89 | ros::NodeHandle& nh = getMTNodeHandle(); 90 | ros::NodeHandle& private_nh = getMTPrivateNodeHandle(); 91 | 92 | private_nh.getParam("min_y", min_y_); 93 | private_nh.getParam("max_y", max_y_); 94 | private_nh.getParam("min_x", min_x_); 95 | private_nh.getParam("max_x", max_x_); 96 | private_nh.getParam("max_z", max_z_); 97 | private_nh.getParam("goal_z", goal_z_); 98 | private_nh.getParam("z_scale", z_scale_); 99 | private_nh.getParam("x_scale", x_scale_); 100 | private_nh.getParam("enabled", enabled_); 101 | 102 | receivedRectCoord = false; 103 | ul_x = ul_y = lr_x = lr_y = 0; 104 | focal_length_ = 575.8157348632812; 105 | goaldepthFlag = odomFlag = false; 106 | 107 | DIST_TO_GOAL_ = 0.5; 108 | TERMINAL_STATE_.isTerminal = true; 109 | episodeNum=0; 110 | stepCnt=0; 111 | MAX_STEP_CNT_ = 50; // Max number of steps the agent can take to find goal 112 | 113 | // Constants for discretizing state space 114 | ANGLE_DIVIDER_ = 24; 115 | DIST_DIVIDER_ = 3; 116 | 117 | 118 | /** Agent's GPS location */ 119 | getmodelstate_ = nh.serviceClient("/gazebo/get_model_state"); 120 | 121 | reinforcement_srv_ = nh.advertiseService("agent_environment", &LileeEnvironment::agentenvironmentcb, this); 122 | respawnerpub_ = private_nh.advertise("/gazebo/set_model_state", 1); 123 | cmdpub_ = private_nh.advertise("/cmd_vel_mux/input/navi", 1); 124 | rectanglesub_ = nh.subscribe("/location_data", 1, &LileeEnvironment::rectanglecb, this); 125 | goaldepthsub_ = nh.subscribe("/camera/depth/image_raw", 1, &LileeEnvironment::goaldepthcb, this); 126 | bumpersub_ = nh.subscribe("/mobile_base/events/bumper", 1, &LileeEnvironment::bumpercb, this); 127 | 128 | respawn_goal(); 129 | respawn_agent(); 130 | } 131 | 132 | 133 | /********************************************************* 134 | * Respawn functions 135 | *********************************************************/ 136 | void respawn_goal() 137 | { // Respawn goal model randomly on the map. 138 | gazebo_msgs::ModelState modelstate; 139 | modelstate.model_name = goal_model_name; 140 | modelstate.reference_frame = "world"; 141 | 142 | geometry_msgs::Pose pose; 143 | pose.position.x = rand() % WORLD_BOUND_; 144 | pose.position.y = rand() % (2*WORLD_BOUND_) - WORLD_BOUND_; 145 | pose.position.z = 0; 146 | pose. orientation.w = 1.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0; 147 | modelstate.pose = pose; 148 | 149 | respawnerpub_.publish(modelstate); 150 | } 151 | void respawn_agent() 152 | { // Respawn agent model to (0,0,0) on the map. 153 | gazebo_msgs::ModelState modelstate; 154 | modelstate.model_name = agent_model_name; 155 | modelstate.reference_frame = "world"; 156 | 157 | geometry_msgs::Pose pose; 158 | 159 | int agent_x, agent_y; 160 | agent_x = agent_y = 2; 161 | while ((agent_x == 2) && (agent_y == 2)) 162 | { 163 | agent_x = (rand() % WORLD_BOUND_); 164 | agent_y = (rand() % WORLD_BOUND_); 165 | } 166 | pose.position.x=agent_x; pose.position.y=agent_y; pose.position.z = 0; pose.orientation.w = (rand() % 200)*(.01)-1; pose.orientation.z = (rand() % 200)*(.01)-1;pose.orientation.x = pose.orientation.y = 0; 167 | 168 | modelstate.pose = pose; 169 | 170 | respawnerpub_.publish(modelstate); 171 | 172 | stepCnt = 0; 173 | episodeNum++; 174 | ROS_ERROR("envir: Episode %d", episodeNum); 175 | } 176 | 177 | // Execute action. 178 | void executeAction(float linear_vel, float angular_vel) 179 | { 180 | geometry_msgs::Twist msg; 181 | msg.linear.x = linear_vel; 182 | msg.angular.z = angular_vel; 183 | cmdpub_.publish(msg); 184 | stepCnt++; 185 | 186 | } 187 | 188 | /********************************************************* 189 | * Communication with agent 190 | *********************************************************/ 191 | bool agentenvironmentcb(lilee_rl_continuous::AgentEnvironment::Request& req, lilee_rl_continuous::AgentEnvironment::Response& res) 192 | { 193 | // perform action 194 | executeAction(req.action.linear_vel, req.action.angular_vel); 195 | ros::Duration(actions::ACTION_DURATION_).sleep(); 196 | 197 | // read next state 198 | goaldepthFlag = odomFlag = false; 199 | while (!(goaldepthFlag && odomFlag)) { ros::Duration(0.1).sleep(); } 200 | 201 | // Send next state and reward 202 | if ((state_.distance_to_goal <= 0.55) && (state_.theta_to_goal <= 0.2)) 203 | { // Goal state 204 | respawn_goal(); 205 | respawn_agent(); 206 | res.reward = 100.0; 207 | res.state = TERMINAL_STATE_; 208 | res.state.success = 1; 209 | } 210 | else if (stepCnt > MAX_STEP_CNT_) 211 | { // If agent took MAX_STEP_CNT steps, restart episode. 212 | ROS_ERROR("envir: Agent failed to find goal after %d steps.", MAX_STEP_CNT_); 213 | respawn_goal(); 214 | respawn_agent(); 215 | res.reward = -1.0; 216 | res.state = state_; 217 | res.state.isTerminal = true; 218 | res.state.success = 0; 219 | } 220 | else 221 | { 222 | res.reward = -0.5*fabs(req.action.linear_vel); //-1.0 - 0.5*fabs(req.action.linear_vel); 223 | res.state = state_; 224 | 225 | } 226 | 227 | return true; 228 | } 229 | 230 | void storeGoalNaN() 231 | { 232 | state_.goal.position_x = std::numeric_limits::quiet_NaN(); 233 | state_.goal.position_z = std::numeric_limits::quiet_NaN(); 234 | 235 | } 236 | void storeGoalXYZ(float position_x, float position_z) 237 | { 238 | state_.goal.position_x = position_x; 239 | state_.goal.position_z = position_z; 240 | } 241 | 242 | void agentPositionCb() // Agent's GPS Location 243 | { 244 | gazebo_msgs::GetModelState agent, goal; 245 | agent.request.model_name = agent_model_name; 246 | goal.request.model_name = goal_model_name; 247 | float abs_theta_to_goal, difference; 248 | 249 | if (!odomFlag) 250 | { 251 | if (getmodelstate_.call(agent) && getmodelstate_.call(goal)) 252 | { 253 | odomFlag = true; 254 | float radians; 255 | float x1 = agent.response.pose.position.x; 256 | float y1 = agent.response.pose.position.y; 257 | float x2 = goal.response.pose.position.x; 258 | float y2 = goal.response.pose.position.y; 259 | 260 | 261 | state_.distance_to_goal = sqrt(pow((x1-x2),2) + pow((y1-y2),2)); 262 | if (x1 < x2) 263 | { 264 | radians=atan((y1-y2)/(x1-x2)); 265 | } 266 | else if (x1 > x2) 267 | { 268 | radians=atan((y1-y2)/(x1-x2)) + M_PI; 269 | 270 | } 271 | else 272 | { 273 | if (y2 > y1) 274 | radians = M_PI/2; 275 | else 276 | radians = 3*M_PI/2; 277 | } 278 | 279 | 280 | if (radians > M_PI) 281 | abs_theta_to_goal = radians - 2*M_PI; 282 | else 283 | abs_theta_to_goal= radians; 284 | 285 | odomFlag = true; 286 | state_.agent_location.position_x = x1; 287 | state_.agent_location.position_y = y1; 288 | 289 | double roll, pitch, yaw; 290 | tf::Quaternion q(agent.response.pose.orientation.x, 291 | agent.response.pose.orientation.y, 292 | agent.response.pose.orientation.z, 293 | agent.response.pose.orientation.w); 294 | tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 295 | 296 | difference = abs_theta_to_goal - yaw; 297 | if (difference > M_PI) 298 | state_.theta_to_goal = difference - 2*M_PI; 299 | else 300 | state_.theta_to_goal = difference; 301 | state_.agent_location.orientation_yaw = yaw; 302 | 303 | state_.isTerminal = false; 304 | } 305 | } 306 | } 307 | 308 | void bumpercb(const kobuki_msgs::BumperEvent::ConstPtr& msg) 309 | { 310 | state_.bumper.bumper = msg->bumper; 311 | state_.bumper.state = msg->state; 312 | 313 | } 314 | 315 | /*!******************************************************* 316 | * @brief Process image data and publish location of object. 317 | *********************************************************/ 318 | // Convert [c1, c2, c3, c4] into a float 319 | float charsToFloat(unsigned char c1, unsigned char c2, unsigned char c3, unsigned char c4, bool is_bigendian) 320 | { 321 | float f; 322 | if ( htonl(47) == 47 ) { 323 | // System is big endian 324 | if (is_bigendian) { 325 | *((char *)(&f)) = c4; 326 | *((char *)(&f)+1) = c3; 327 | *((char *)(&f)+2) = c2; 328 | *((char *)(&f)+3) = c1; 329 | } 330 | else { 331 | *((char *)(&f)) = c1; 332 | *((char *)(&f)+1) = c2; 333 | *((char *)(&f)+2) = c3; 334 | *((char *)(&f)+3) = c4; 335 | } 336 | } 337 | else { 338 | //System is little endian 339 | if (!is_bigendian) { 340 | *((char *)(&f)) = c1; 341 | *((char *)(&f)+1) = c2; 342 | *((char *)(&f)+2) = c3; 343 | *((char *)(&f)+3) = c4; 344 | } 345 | else { 346 | *((char *)(&f)) = c4; 347 | *((char *)(&f)+1) = c3; 348 | *((char *)(&f)+2) = c2; 349 | *((char *)(&f)+3) = c1; 350 | } 351 | } 352 | return f; 353 | } 354 | 355 | // Callback for rectangle coordinates. 356 | void rectanglecb(const std_msgs::String::ConstPtr& msg){ 357 | int temp_ul_x, temp_ul_y, temp_lr_x, temp_lr_y; 358 | sscanf(msg->data.c_str(), "(%d, %d),(%d, %d)", &temp_ul_x, &temp_ul_y, &temp_lr_x, &temp_lr_y); 359 | 360 | ul_x = temp_ul_x; 361 | ul_y = temp_ul_y; 362 | lr_x = temp_lr_x; 363 | lr_y = temp_lr_y; 364 | 365 | receivedRectCoord = true; 366 | } 367 | 368 | //Callback for /camera/depth/image. 369 | void goaldepthcb(const sensor_msgs::ImageConstPtr& msg) 370 | { 371 | agentPositionCb(); // Agent's GPS location 372 | 373 | if (!goaldepthFlag) { 374 | goaldepthFlag = true; 375 | 376 | // Goal location 377 | if (!receivedRectCoord) 378 | { 379 | storeGoalNaN(); 380 | } 381 | else 382 | { 383 | receivedRectCoord = false; 384 | float x = 0.0, y = 0.0, z = 1e6, temp_x, temp_y, temp_z; 385 | unsigned int n = 0; 386 | 387 | // for each (x, y) inside rectangle: 388 | for (int i = ul_x; i < lr_x; i++) for (int j = ul_y; j < lr_y; j++) 389 | { 390 | int index = j*4*(msg->width) + i*4; 391 | temp_z = charsToFloat(msg->data[index], msg->data[index+1], msg->data[index+2], msg->data[index+3], msg->is_bigendian); 392 | if (!std::isnan(temp_z) && temp_z > 0.0) 393 | { 394 | temp_x = (i-320)*temp_z/focal_length_; 395 | temp_y = (j-240)*temp_z/focal_length_; 396 | if (-temp_y > min_y_ && -temp_y < max_y_ && temp_x < max_x_ && temp_x > min_x_ && temp_z < max_z_) 397 | { 398 | x += temp_x; 399 | y += temp_y; 400 | z = std::min(z, temp_z); 401 | n++; 402 | } 403 | } 404 | } 405 | if (n>100) 406 | { 407 | x /= n; 408 | y /= n; 409 | if(z > max_z_) 410 | { 411 | if (enabled_) 412 | storeGoalNaN(); 413 | return; 414 | } 415 | if (enabled_) 416 | storeGoalXYZ(x,z); 417 | } 418 | else 419 | { 420 | if (enabled_) 421 | storeGoalNaN(); 422 | } 423 | } // end else 424 | 425 | } // end if(receivedAction) 426 | } // end goaldepthcb 427 | 428 | ros::Subscriber rectanglesub_, goaldepthsub_, odomsub_, bumpersub_; 429 | ros::Publisher cmdpub_, respawnerpub_; 430 | }; 431 | 432 | PLUGINLIB_DECLARE_CLASS(lilee_rl_continuous, LileeEnvironment, lilee_rl_continuous::LileeEnvironment, nodelet::Nodelet); 433 | 434 | } 435 | -------------------------------------------------------------------------------- /lilee_rl_continuous/src/teleop.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "boost/thread/mutex.hpp" 7 | #include "boost/thread/thread.hpp" 8 | #include "lilee_rl_continuous/actions.h" 9 | #include "lilee_rl_continuous/TeleopCmd.h" 10 | 11 | #define KEYCODE_R 0x43 12 | #define KEYCODE_L 0x44 13 | #define KEYCODE_U 0x41 14 | #define KEYCODE_D 0x42 15 | #define KEYCODE_Q 0x71 16 | 17 | class LileeTeleop 18 | { 19 | public: 20 | LileeTeleop(); 21 | void keyLoop(); 22 | void watchdog(); 23 | 24 | private: 25 | 26 | ros::NodeHandle nh_,ph_; 27 | double linear_, angular_; 28 | ros::Time first_publish_; 29 | ros::Time last_publish_; 30 | double l_scale_, a_scale_; 31 | ros::Publisher cmdpub_, teleopcmdpub_; 32 | void publish(double, double); 33 | void sendTeleopCmd(float, float); 34 | void setUserControl(); 35 | boost::mutex publish_mutex_; 36 | 37 | bool userControl; 38 | }; 39 | 40 | LileeTeleop::LileeTeleop(): 41 | ph_("~"), 42 | linear_(0), 43 | angular_(0), 44 | l_scale_(1.0), 45 | a_scale_(1.0) 46 | { 47 | ph_.param("scale_angular", a_scale_, a_scale_); 48 | ph_.param("scale_linear", l_scale_, l_scale_); 49 | 50 | cmdpub_ = nh_.advertise("/cmd_vel_mux/input/navi", 1); 51 | teleopcmdpub_ = nh_.advertise("teleop_cmd",1); 52 | 53 | userControl = true; 54 | } 55 | 56 | int kfd = 0; 57 | struct termios cooked, raw; 58 | 59 | void quit(int sig) 60 | { 61 | tcsetattr(kfd, TCSANOW, &cooked); 62 | ros::shutdown(); 63 | exit(0); 64 | } 65 | 66 | 67 | int main(int argc, char** argv) 68 | { 69 | ros::init(argc, argv, "teleop"); 70 | LileeTeleop Lilee_teleop; 71 | ros::NodeHandle n; 72 | 73 | signal(SIGINT,quit); 74 | 75 | boost::thread my_thread(boost::bind(&LileeTeleop::keyLoop, &Lilee_teleop)); 76 | ros::spin(); 77 | 78 | my_thread.interrupt() ; 79 | my_thread.join() ; 80 | return(0); 81 | } 82 | 83 | 84 | void LileeTeleop::watchdog() 85 | { 86 | boost::mutex::scoped_lock lock(publish_mutex_); 87 | if ((ros::Time::now() > last_publish_ + ros::Duration(0.15)) && 88 | (ros::Time::now() > first_publish_ + ros::Duration(0.50))) 89 | publish(0, 0); 90 | } 91 | 92 | void LileeTeleop::sendTeleopCmd(float linear_vel, float angular_vel) 93 | { 94 | lilee_rl_continuous::TeleopCmd msg; 95 | msg.user_control = userControl; 96 | msg.action.linear_vel = linear_vel; 97 | msg.action.angular_vel = angular_vel; 98 | 99 | teleopcmdpub_.publish(msg); 100 | } 101 | 102 | void LileeTeleop::setUserControl() 103 | { 104 | if (userControl) 105 | { 106 | userControl = false; 107 | 108 | } 109 | else 110 | { 111 | userControl = true; 112 | } 113 | lilee_rl_continuous::TeleopCmd msg; 114 | msg.user_control = userControl; 115 | teleopcmdpub_.publish(msg); 116 | } 117 | 118 | void LileeTeleop::keyLoop() 119 | { 120 | char c; 121 | 122 | // get the console in raw mode 123 | tcgetattr(kfd, &cooked); 124 | memcpy(&raw, &cooked, sizeof(struct termios)); 125 | raw.c_lflag &=~ (ICANON | ECHO); 126 | // Setting a new line, then end of file 127 | raw.c_cc[VEOL] = 1; 128 | raw.c_cc[VEOF] = 2; 129 | tcsetattr(kfd, TCSANOW, &raw); 130 | 131 | puts("Reading from keyboard"); 132 | puts("---------------------------"); 133 | puts("Use arrow keys to move the Turtlebot."); 134 | 135 | 136 | while (ros::ok()) 137 | { 138 | // get the next event from the keyboard 139 | if(read(kfd, &c, 1) < 0) 140 | { 141 | perror("read():"); 142 | exit(-1); 143 | } 144 | 145 | linear_=angular_=0; 146 | ROS_DEBUG("value: 0x%02X\n", c); 147 | 148 | switch(c) 149 | { 150 | case KEYCODE_L: 151 | sendTeleopCmd(0.0, 0.5); 152 | break; 153 | case KEYCODE_R: 154 | sendTeleopCmd(0.0, -0.5); 155 | break; 156 | case KEYCODE_U: 157 | sendTeleopCmd(1.0, 0.0); 158 | break; 159 | case KEYCODE_D: 160 | setUserControl(); 161 | break; 162 | } 163 | 164 | boost::mutex::scoped_lock lock(publish_mutex_); 165 | if (ros::Time::now() > last_publish_ + ros::Duration(1.0)) { 166 | first_publish_ = ros::Time::now(); 167 | } 168 | last_publish_ = ros::Time::now(); 169 | } 170 | return; 171 | } 172 | 173 | void LileeTeleop::publish(double angular, double linear) 174 | { 175 | geometry_msgs::Twist vel; 176 | vel.angular.z = a_scale_*angular; 177 | vel.linear.x = l_scale_*linear; 178 | 179 | cmdpub_.publish(vel); 180 | 181 | return; 182 | } 183 | -------------------------------------------------------------------------------- /lilee_rl_continuous/src/world_control.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include //std::string 3 | 4 | #include "gazebo_msgs/SpawnModel.h" 5 | #include "gazebo_msgs/DeleteModel.h" 6 | #include "std_srvs/Empty.h" 7 | #include "lilee_rl_continuous/WorldControl.h" 8 | 9 | namespace world_control 10 | { 11 | int NUM_WORLDS_ = 6; 12 | } 13 | 14 | ros::ServiceClient pause_client, unpause_client, gazebo_spawn_client, gazebo_delete_client; 15 | ros::ServiceServer worldcontrolsrv_; 16 | 17 | /********************************************************* 18 | * Pause/Unpause world 19 | *********************************************************/ 20 | int pauseWorld() 21 | { 22 | std_srvs::Empty empty; 23 | if (!pause_client.call(empty)) 24 | { 25 | ROS_ERROR("unable to pause physics engine"); 26 | return -1; 27 | } 28 | return 0; 29 | } 30 | 31 | int unpauseWorld() 32 | { 33 | std_srvs::Empty empty; 34 | if (!unpause_client.call(empty)) 35 | { 36 | ROS_ERROR("unable to unpause physics engine"); 37 | return -1; 38 | } 39 | return 0; 40 | } 41 | 42 | /********************************************************* 43 | * Change friction of 'ground_plane' model to (mu, mu2) 44 | *********************************************************/ 45 | int changeFriction(double mu, double mu2) 46 | { 47 | pauseWorld(); 48 | 49 | char buffer [780]; 50 | sprintf(buffer, 51 | "10 0 1100 100%f%f1000 0 1100 10000001", 52 | mu, mu2 53 | ); 54 | 55 | // Delete 'ground_plane' 56 | gazebo_msgs::DeleteModel deletemodel; 57 | deletemodel.request.model_name = "ground_plane"; 58 | gazebo_delete_client.call(deletemodel); 59 | 60 | // respawn new 'ground_plane' 61 | gazebo_msgs::SpawnModel model; 62 | model.request.model_xml = buffer; 63 | 64 | 65 | model.request.model_name="ground_plane"; 66 | model.request.reference_frame="world"; 67 | if (!gazebo_spawn_client.call(model)) 68 | { 69 | ROS_ERROR("world_control: Failed to respawn new 'ground_plane'"); 70 | return -1; 71 | } 72 | else 73 | ROS_ERROR("world_control: Changed friction to (%f, %f)", mu, mu2); 74 | 75 | unpauseWorld(); 76 | 77 | return 0; 78 | } 79 | 80 | int changeWorld(int numWorld) 81 | { // Change world to numWorld 82 | 83 | if (numWorld > world_control::NUM_WORLDS_) 84 | { 85 | ROS_ERROR("world_control: Invalid world number"); 86 | return -1; 87 | } 88 | 89 | if (numWorld == 1) 90 | changeFriction(100, 50); 91 | else if (numWorld == 2) 92 | changeFriction(5,5); 93 | else if (numWorld == 5) 94 | changeFriction(0.2, 0.2); 95 | else if (numWorld == 3) 96 | changeFriction(10, 0.1); 97 | else if (numWorld == 4) 98 | changeFriction(0.1, 50); 99 | else if (numWorld == 6) 100 | changeFriction(3,1.5); 101 | 102 | return 0; 103 | } 104 | /********************************************************* 105 | * Callback 106 | *********************************************************/ 107 | bool worldcontrolcb(lilee_rl_continuous::WorldControl::Request& req, lilee_rl_continuous::WorldControl::Response& res) 108 | { 109 | changeWorld((int)req.numWorld); 110 | return true; 111 | } 112 | 113 | /********************************************************* 114 | * Main 115 | *********************************************************/ 116 | int main(int argc, char **argv) 117 | { 118 | ros::init(argc, argv, "lilee_sample"); 119 | ros::NodeHandle n; 120 | 121 | pause_client = n.serviceClient("/gazebo/pause_physics"); 122 | pause_client.waitForExistence(); 123 | 124 | unpause_client = n.serviceClient("/gazebo/unpause_physics"); 125 | unpause_client.waitForExistence(); 126 | 127 | gazebo_spawn_client = n.serviceClient ("/gazebo/spawn_sdf_model"); 128 | gazebo_spawn_client.waitForExistence(); 129 | 130 | gazebo_delete_client = n.serviceClient ("/gazebo/delete_model"); 131 | gazebo_delete_client.waitForExistence(); 132 | 133 | worldcontrolsrv_ = n.advertiseService("/world_control", worldcontrolcb); 134 | 135 | ros::spin(); 136 | 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /lilee_rl_continuous/srv/AgentEnvironment.srv: -------------------------------------------------------------------------------- 1 | lilee_rl_continuous/ContinuousAction action 2 | --- 3 | lilee_rl_continuous/State state 4 | float32 reward 5 | -------------------------------------------------------------------------------- /lilee_rl_continuous/srv/WorldControl.srv: -------------------------------------------------------------------------------- 1 | int16 numWorld 2 | --- 3 | -------------------------------------------------------------------------------- /lilee_rl_discrete/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lilee_rl_discrete) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | message_generation 9 | roscpp 10 | rospy 11 | std_msgs 12 | nodelet 13 | pcl_ros 14 | turtlebot_msgs 15 | kobuki_msgs 16 | geometry_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | find_package(Boost REQUIRED) 21 | find_package(PCL) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 40 | ## pulled in transitively but can be declared for certainty nonetheless: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | GoalLocation.msg 57 | State.msg 58 | AgentLocation.msg 59 | TeleopCmd.msg 60 | ) 61 | 62 | ## Generate services in the 'srv' folder 63 | add_service_files( 64 | FILES 65 | AgentEnvironment.srv 66 | ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | generate_messages( 77 | DEPENDENCIES 78 | std_msgs 79 | kobuki_msgs 80 | ) 81 | 82 | ################################### 83 | ## catkin specific configuration ## 84 | ################################### 85 | ## The catkin_package macro generates cmake config files for your package 86 | ## Declare things to be passed to dependent projects 87 | ## INCLUDE_DIRS: uncomment this if you package contains header files 88 | ## LIBRARIES: libraries you create in this project that dependent projects also need 89 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 90 | ## DEPENDS: system dependencies of this project that dependent projects also need 91 | catkin_package( 92 | INCLUDE_DIRS 93 | LIBRARIES ${PROJECT_NAME} turtlebot_teleop 94 | CATKIN_DEPENDS nodelet pcl_ros roscpp turtlebot_msgs kobuki_msgs geometry_msgs 95 | DEPENDS Boost PCL 96 | ) 97 | 98 | ########### 99 | ## Build ## 100 | ########### 101 | 102 | ## Specify additional locations of header files 103 | ## Your package locations should be listed before other locations 104 | # include_directories(include) 105 | include_directories( 106 | include 107 | ${catkin_INCLUDE_DIRS} 108 | ${PCL_INCLUDE_DIRS} 109 | ${Boost_INCLUDE_DIRS} 110 | ) 111 | 112 | ## Declare a cpp library 113 | add_library(${PROJECT_NAME} src/environment.cpp src/actions.cpp) 114 | 115 | add_dependencies(${PROJECT_NAME} turtlebot_msgs_gencpp) 116 | add_dependencies(${PROJECT_NAME} kobuki_msgs_gencpp) 117 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) 118 | add_dependencies(${PROJECT_NAME} lilee_rl_discrete_generate_messages_cpp) 119 | #add_dependencies(${PROJECT_NAME} lilee_rl_discrete_gencpp) 120 | 121 | target_link_libraries(${PROJECT_NAME} 122 | ${catkin_LIBRARIES} 123 | ${PCL_LIBRARIES} 124 | ) 125 | 126 | add_executable(agent_qLearning src/agent_qLearning.cpp) 127 | target_link_libraries(agent_qLearning ${PROJECT_NAME} ${catkin_LIBRARIES}) 128 | add_dependencies(agent_qLearning lilee_rl_discrete_generate_messages_cpp) 129 | 130 | add_executable(discreteTeleop src/teleop.cpp src/actions.cpp) 131 | target_link_libraries(discreteTeleop ${catkin_LIBRARIES}) 132 | add_dependencies(discreteTeleop lilee_rl_discrete_generate_messages_cpp) 133 | 134 | ## Declare a cpp executable 135 | # add_executable(lilee_agent_node src/lilee_agent_node.cpp) 136 | 137 | ## Add cmake target dependencies of the executable/library 138 | ## as an example, message headers may need to be generated before nodes 139 | # add_dependencies(lilee_agent_node lilee_agent_generate_messages_cpp) 140 | 141 | ## Specify libraries to link a library or executable target against 142 | 143 | 144 | ############# 145 | ## Install ## 146 | ############# 147 | 148 | # all install targets should use catkin DESTINATION variables 149 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 150 | 151 | ## Mark executable scripts (Python etc.) for installation 152 | ## in contrast to setup.py, you can choose the destination 153 | # install(PROGRAMS 154 | # scripts/my_python_script 155 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 156 | # ) 157 | 158 | ## Mark executables and/or libraries for installation 159 | 160 | install(TARGETS ${PROJECT_NAME} 161 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | ) 165 | 166 | install(TARGETS lilee_rl_discrete 167 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | ) 171 | 172 | ## Mark cpp header files for installation 173 | # install(DIRECTORY include/${PROJECT_NAME}/ 174 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 175 | # FILES_MATCHING PATTERN "*.h" 176 | # PATTERN ".svn" EXCLUDE 177 | # ) 178 | 179 | ## Mark other files for installation (e.g. launch and bag files, etc.) 180 | install(DIRECTORY sim 181 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 182 | ) 183 | 184 | install(DIRECTORY launch 185 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | ) 187 | 188 | install(DIRECTORY param 189 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | ) 191 | 192 | install(DIRECTORY plugins 193 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lilee_agent.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | 209 | 210 | 211 | 212 | -------------------------------------------------------------------------------- /lilee_rl_discrete/include/lilee_rl_discrete/actions.h: -------------------------------------------------------------------------------- 1 | #ifndef ACTIONS_H_ 2 | #define ACTIONS_H_ 3 | #include "geometry_msgs/Twist.h" 4 | 5 | namespace actions { 6 | extern int NUM_ACTIONS_; 7 | extern double ACTION_DURATION_; 8 | 9 | enum Move 10 | { 11 | FORWARD_ = 0, 12 | //BACKWARD_ = 1, 13 | //FORWARD_LEFT_ = 2, 14 | //FORWARD_RIGHT_ = 3, 15 | TURN_LEFT_ = 1, 16 | TURN_RIGHT_ = 2, 17 | //BACKWARD_LEFT_ = 6, 18 | //BACKWARD_RIGHT_ = 7 19 | STOP_ = 3//NUM_ACTIONS_ 20 | }; 21 | 22 | geometry_msgs::TwistPtr move_forward(); 23 | geometry_msgs::TwistPtr move_turnleft(); 24 | geometry_msgs::TwistPtr move_turnright(); 25 | geometry_msgs::TwistPtr stop(); 26 | geometry_msgs::TwistPtr executeAction(int action); 27 | 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /lilee_rl_discrete/include/lilee_rl_discrete/gazebo_parameters.h: -------------------------------------------------------------------------------- 1 | #ifndef GAZEBO_PARAMETERS_H_ 2 | #define GAZEBO_PARAMETERS_H_ 3 | 4 | int WORLD_BOUND_ = 8; // size of world 5 | 6 | int AGENT_INIT_POS_X = 0; 7 | int AGENT_INIT_POS_Y = 0; 8 | int AGENT_INIT_POS_Z = 0; 9 | 10 | std::string goal_model_name = "fire_hydrant"; 11 | std::string agent_model_name = "mobile_base"; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /lilee_rl_discrete/launch/includes/velocity_smoother.launch.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /lilee_rl_discrete/launch/qlearning.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /lilee_rl_discrete/msg/AgentLocation.msg: -------------------------------------------------------------------------------- 1 | float64 position_x 2 | float64 position_y 3 | float64 orientation_yaw 4 | -------------------------------------------------------------------------------- /lilee_rl_discrete/msg/GoalLocation.msg: -------------------------------------------------------------------------------- 1 | float32 position_x 2 | float32 position_z 3 | -------------------------------------------------------------------------------- /lilee_rl_discrete/msg/State.msg: -------------------------------------------------------------------------------- 1 | lilee_rl_discrete/GoalLocation goal 2 | lilee_rl_discrete/AgentLocation agent_location 3 | kobuki_msgs/BumperEvent bumper 4 | int32 distance_to_goal 5 | int32 theta_to_goal 6 | bool isTerminal 7 | bool success 8 | -------------------------------------------------------------------------------- /lilee_rl_discrete/msg/TeleopCmd.msg: -------------------------------------------------------------------------------- 1 | bool user_control 2 | int8 action 3 | -------------------------------------------------------------------------------- /lilee_rl_discrete/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lilee_rl_discrete 4 | 0.0.0 5 | The lilee_rl_discrete package 6 | 7 | 8 | 9 | 10 | Lisa Lee 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | nodelet 45 | pcl_ros 46 | message_generation 47 | roscpp 48 | rospy 49 | std_msgs 50 | turtlebot_msgs 51 | kobuki_msgs 52 | cv_bridge 53 | sensor_msgs 54 | image_transport 55 | geometry_msgs 56 | 57 | roscpp 58 | rospy 59 | std_msgs 60 | nodelet 61 | pcl_ros 62 | turtlebot_msgs 63 | kobuki_msgs 64 | cv_bridge 65 | sensor_msgs 66 | image_transport 67 | geometry_msgs 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /lilee_rl_discrete/param/mux.yaml: -------------------------------------------------------------------------------- 1 | # Created on: Dec 5, 2012 2 | # Author: jorge 3 | # Configuration for subscribers to cmd_vel sources. 4 | # 5 | # Individual subscriber configuration: 6 | # name: Source name 7 | # topic: The topic that provides cmd_vel messages 8 | # timeout: Time in seconds without incoming messages to consider this topic inactive 9 | # priority: Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 10 | # short_desc: Short description (optional) 11 | 12 | subscribers: 13 | - name: "Safe reactive controller" 14 | topic: "input/safety_controller" 15 | timeout: 0.2 16 | priority: 10 17 | - name: "Reinforcement" 18 | topic: "input/reinforcement" 19 | timeout: 0.5 20 | priority: 7 21 | -------------------------------------------------------------------------------- /lilee_rl_discrete/plugins/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The turtlebot people reinforcement node. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /lilee_rl_discrete/src/actions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "geometry_msgs/Twist.h" 3 | #include "lilee_rl_discrete/actions.h" // parameters for agent's actions 4 | 5 | namespace actions { 6 | int NUM_ACTIONS_ = 3; // Number of discrete actions 7 | 8 | double ACTION_DURATION_ = 0.5; // duration of each action (sec) 9 | double LINEAR_X_ = 1; // linear.x velocity 10 | double ANGULAR_Z = 0.45; 11 | 12 | /********************************************************* 13 | * Agent's actions 14 | *********************************************************/ 15 | geometry_msgs::TwistPtr move_forward() 16 | { 17 | geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist()); 18 | cmd->linear.x = LINEAR_X_; 19 | cmd->angular.z = 0.0; 20 | 21 | return cmd; 22 | } 23 | geometry_msgs::TwistPtr move_turnleft() 24 | { 25 | geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist()); 26 | cmd->linear.x = 0.0; 27 | cmd->angular.z = ANGULAR_Z; 28 | return cmd; 29 | } 30 | geometry_msgs::TwistPtr move_turnright() 31 | { 32 | geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist()); 33 | cmd->linear.x = 0.0; 34 | cmd->angular.z = -1*ANGULAR_Z; 35 | return cmd; 36 | } 37 | geometry_msgs::TwistPtr stop() 38 | { 39 | geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist()); 40 | return cmd; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /lilee_rl_discrete/src/teleop.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "boost/thread/mutex.hpp" 7 | #include "boost/thread/thread.hpp" 8 | #include "lilee_rl_discrete/actions.h" 9 | #include "lilee_rl_discrete/TeleopCmd.h" 10 | 11 | #define KEYCODE_R 0x43 12 | #define KEYCODE_L 0x44 13 | #define KEYCODE_U 0x41 14 | #define KEYCODE_D 0x42 15 | #define KEYCODE_Q 0x71 16 | 17 | class LileeTeleop 18 | { 19 | public: 20 | LileeTeleop(); 21 | void keyLoop(); 22 | void watchdog(); 23 | 24 | private: 25 | 26 | ros::NodeHandle nh_,ph_; 27 | double linear_, angular_; 28 | ros::Time first_publish_; 29 | ros::Time last_publish_; 30 | double l_scale_, a_scale_; 31 | ros::Publisher cmdpub_, teleopcmdpub_; 32 | void publish(double, double); 33 | void sendTeleopCmd(int); 34 | void setUserControl(); 35 | boost::mutex publish_mutex_; 36 | 37 | bool userControl; 38 | 39 | }; 40 | 41 | LileeTeleop::LileeTeleop(): 42 | ph_("~"), 43 | linear_(0), 44 | angular_(0), 45 | l_scale_(1.0), 46 | a_scale_(1.0) 47 | { 48 | ph_.param("scale_angular", a_scale_, a_scale_); 49 | ph_.param("scale_linear", l_scale_, l_scale_); 50 | 51 | cmdpub_ = nh_.advertise("/cmd_vel_mux/input/navi", 1); 52 | teleopcmdpub_ = nh_.advertise("teleop_cmd",1); 53 | 54 | userControl = true; 55 | } 56 | 57 | int kfd = 0; 58 | struct termios cooked, raw; 59 | 60 | void quit(int sig) 61 | { 62 | tcsetattr(kfd, TCSANOW, &cooked); 63 | ros::shutdown(); 64 | exit(0); 65 | } 66 | 67 | 68 | int main(int argc, char** argv) 69 | { 70 | ros::init(argc, argv, "teleop"); 71 | LileeTeleop Lilee_teleop; 72 | ros::NodeHandle n; 73 | 74 | signal(SIGINT,quit); 75 | 76 | boost::thread my_thread(boost::bind(&LileeTeleop::keyLoop, &Lilee_teleop)); 77 | 78 | ros::spin(); 79 | 80 | my_thread.interrupt() ; 81 | my_thread.join() ; 82 | return(0); 83 | } 84 | 85 | 86 | void LileeTeleop::watchdog() 87 | { 88 | boost::mutex::scoped_lock lock(publish_mutex_); 89 | if ((ros::Time::now() > last_publish_ + ros::Duration(0.15)) && 90 | (ros::Time::now() > first_publish_ + ros::Duration(0.50))) 91 | publish(0, 0); 92 | } 93 | 94 | void LileeTeleop::sendTeleopCmd(int action) 95 | { 96 | lilee_rl_discrete::TeleopCmd msg; 97 | msg.action = action; 98 | msg.user_control = userControl; 99 | 100 | teleopcmdpub_.publish(msg); 101 | } 102 | 103 | void LileeTeleop::setUserControl() 104 | { 105 | if (userControl) 106 | { 107 | userControl = false; 108 | 109 | } 110 | else 111 | { 112 | userControl = true; 113 | } 114 | lilee_rl_discrete::TeleopCmd msg; 115 | msg.user_control = userControl; 116 | teleopcmdpub_.publish(msg); 117 | } 118 | 119 | void LileeTeleop::keyLoop() 120 | { 121 | char c; 122 | 123 | // get the console in raw mode 124 | tcgetattr(kfd, &cooked); 125 | memcpy(&raw, &cooked, sizeof(struct termios)); 126 | raw.c_lflag &=~ (ICANON | ECHO); 127 | // Setting a new line, then end of file 128 | raw.c_cc[VEOL] = 1; 129 | raw.c_cc[VEOF] = 2; 130 | tcsetattr(kfd, TCSANOW, &raw); 131 | 132 | puts("Reading from keyboard"); 133 | puts("---------------------------"); 134 | puts("Use arrow keys to move the Turtlebot."); 135 | 136 | 137 | while (ros::ok()) 138 | { 139 | // get the next event from the keyboard 140 | if(read(kfd, &c, 1) < 0) 141 | { 142 | perror("read():"); 143 | exit(-1); 144 | } 145 | 146 | linear_=angular_=0; 147 | ROS_DEBUG("value: 0x%02X\n", c); 148 | 149 | switch(c) 150 | { 151 | case KEYCODE_L: 152 | sendTeleopCmd(actions::TURN_LEFT_); 153 | break; 154 | case KEYCODE_R: 155 | sendTeleopCmd(actions::TURN_RIGHT_); 156 | break; 157 | case KEYCODE_U: 158 | sendTeleopCmd(actions::FORWARD_); 159 | break; 160 | case KEYCODE_D: 161 | setUserControl(); 162 | break; 163 | } 164 | 165 | boost::mutex::scoped_lock lock(publish_mutex_); 166 | if (ros::Time::now() > last_publish_ + ros::Duration(1.0)) { 167 | first_publish_ = ros::Time::now(); 168 | } 169 | last_publish_ = ros::Time::now(); 170 | } 171 | return; 172 | } 173 | 174 | void LileeTeleop::publish(double angular, double linear) 175 | { 176 | geometry_msgs::Twist vel; 177 | vel.angular.z = a_scale_*angular; 178 | vel.linear.x = l_scale_*linear; 179 | 180 | cmdpub_.publish(vel); 181 | 182 | 183 | return; 184 | } 185 | -------------------------------------------------------------------------------- /lilee_rl_discrete/srv/AgentEnvironment.srv: -------------------------------------------------------------------------------- 1 | int8 action 2 | --- 3 | lilee_rl_discrete/State state 4 | float32 reward 5 | -------------------------------------------------------------------------------- /lilee_tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lilee_tracker) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | cv_bridge 9 | rospy 10 | sensor_msgs 11 | std_msgs 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 34 | ## pulled in transitively but can be declared for certainty nonetheless: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # sensor_msgs# std_msgs 72 | # ) 73 | 74 | ################################### 75 | ## catkin specific configuration ## 76 | ################################### 77 | ## The catkin_package macro generates cmake config files for your package 78 | ## Declare things to be passed to dependent projects 79 | ## INCLUDE_DIRS: uncomment this if you package contains header files 80 | ## LIBRARIES: libraries you create in this project that dependent projects also need 81 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 82 | ## DEPENDS: system dependencies of this project that dependent projects also need 83 | catkin_package( 84 | # INCLUDE_DIRS include 85 | # LIBRARIES lilee_tracker 86 | # CATKIN_DEPENDS cv_bridge rospy sensor_msgs std_msgs 87 | # DEPENDS system_lib 88 | ) 89 | 90 | ########### 91 | ## Build ## 92 | ########### 93 | 94 | ## Specify additional locations of header files 95 | ## Your package locations should be listed before other locations 96 | # include_directories(include) 97 | include_directories( 98 | ${catkin_INCLUDE_DIRS} 99 | ) 100 | 101 | ## Declare a cpp library 102 | # add_library(lilee_tracker 103 | # src/${PROJECT_NAME}/lilee_tracker.cpp 104 | # ) 105 | 106 | ## Declare a cpp executable 107 | # add_executable(lilee_tracker_node src/lilee_tracker_node.cpp) 108 | 109 | ## Add cmake target dependencies of the executable/library 110 | ## as an example, message headers may need to be generated before nodes 111 | # add_dependencies(lilee_tracker_node lilee_tracker_generate_messages_cpp) 112 | 113 | ## Specify libraries to link a library or executable target against 114 | # target_link_libraries(lilee_tracker_node 115 | # ${catkin_LIBRARIES} 116 | # ) 117 | 118 | ############# 119 | ## Install ## 120 | ############# 121 | 122 | # all install targets should use catkin DESTINATION variables 123 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 124 | 125 | ## Mark executable scripts (Python etc.) for installation 126 | ## in contrast to setup.py, you can choose the destination 127 | # install(PROGRAMS 128 | # scripts/my_python_script 129 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 130 | # ) 131 | 132 | ## Mark executables and/or libraries for installation 133 | # install(TARGETS lilee_tracker lilee_tracker_node 134 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 135 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 136 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 137 | # ) 138 | 139 | ## Mark cpp header files for installation 140 | # install(DIRECTORY include/${PROJECT_NAME}/ 141 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 142 | # FILES_MATCHING PATTERN "*.h" 143 | # PATTERN ".svn" EXCLUDE 144 | # ) 145 | 146 | ## Mark other files for installation (e.g. launch and bag files, etc.) 147 | # install(FILES 148 | # # myfile1 149 | # # myfile2 150 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 151 | # ) 152 | 153 | ############# 154 | ## Testing ## 155 | ############# 156 | 157 | ## Add gtest based cpp test target and link libraries 158 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lilee_tracker.cpp) 159 | # if(TARGET ${PROJECT_NAME}-test) 160 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 161 | # endif() 162 | 163 | ## Add folders to be run by python nosetests 164 | # catkin_add_nosetests(test) 165 | -------------------------------------------------------------------------------- /lilee_tracker/lilee_tracker.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RLAgent/Robotics-RL/8f509f38ed94c005561c14ed8f60b65066d9e3c9/lilee_tracker/lilee_tracker.jpg -------------------------------------------------------------------------------- /lilee_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lilee_tracker 4 | 0.0.0 5 | The lilee_tracker package 6 | 7 | 8 | 9 | 10 | lilee 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | cv_bridge 45 | rospy 46 | sensor_msgs 47 | std_msgs 48 | 49 | cv_bridge 50 | rospy 51 | sensor_msgs 52 | std_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /lilee_tracker/scripts/thresh.txt: -------------------------------------------------------------------------------- 1 | { 2 | 'low_red': 148, 3 | 'high_red': 250, 4 | 'low_green': 62, 5 | 'high_green': 245, 6 | 'low_blue': 24, 7 | 'high_blue': 99, 8 | 'low_hue': 73, 9 | 'high_hue': 112, 10 | 'low_sat': 121, 11 | 'high_sat': 222, 12 | 'low_val': 148, 13 | 'high_val': 250 14 | } -------------------------------------------------------------------------------- /readme: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # About # 3 | ################################################################################ 4 | 5 | Author: Lisa Lee 6 | Advisor/mentors: Dr. Eric Eaton, Dr. Haitham Bou Ammar, Christopher Clingerman 7 | 8 | This work was supported by the National Science Foundation REU Site at the 9 | GRASP Laboratory in the University of Pennsylvania. 10 | 11 | My research paper can be found at www.princeton.edu/~lilee/reu_paper.pdf 12 | 13 | ################################################################################ 14 | # List of ROS packages # 15 | ################################################################################ 16 | 17 | * lilee_follower: Follows object inside the rectangle box published in 18 | /location_data. (See https://youtu.be/bV7wMxJ8f7s) 19 | 20 | * lilee_gazebo: Spawns a turtlebot in gazebo. 21 | 22 | * lilee_rl_continuous: Simulates policy gradient (PG) learning and PG-ELLA in 23 | gazebo. The state and action spaces are continuous. 24 | 25 | * lilee_rl_discrete: Simulates Q-Learning in gazebo. The state and action 26 | spaces are discrete. 27 | 28 | * lilee_tracker: Tracks an object whose color matches rgbhsv values in 29 | thresh.txt, and publishes a rectangle box surrounding that object (on the 30 | camera screen) to /location_data. (See lilee_tracker/lilee_tracker.jpg) 31 | 32 | ################################################################################ 33 | # Launch instructions # 34 | ################################################################################ 35 | 36 | * Q-Learning (gazebo) 37 | 1. roslaunch lilee_gazebo launch_maze_qlearning.launch 38 | 2. roslaunch lilee_rl_discrete qlearning.launch 39 | 40 | * PG (gazebo) 41 | 1. roslaunch lilee_gazebo launch_maze_pg.launch 42 | 2. In MATLAB: 43 | i. Add LifelongRL-Robotics/lilee/MATLAB to path 44 | ii. Run pg_startup 45 | 3. roslaunch lilee_rl_continuous pg.launch 46 | 47 | * PG-ELLA (gazebo) 48 | 1. roslaunch lilee_gazebo launch_maze_pg.launch 49 | 2. In MATLAB: 50 | i. Add LifelongRL-Robotics/lilee/MATLAB to path 51 | ii. Run pg_ella_startup 52 | 3. roslaunch lilee_rl_continuous pg-ella.launch 53 | 54 | * Edit maze.world: 55 | - For Q-learning: 56 | 1. roslaunch lilee_gazebo edit_maze_qlearning.launch 57 | 2. Modify the world in gazebo 58 | 3. "Save As..." lilee_gazebo/worlds/maze_qlearning.world 59 | - For PG/PG-ELLA: 60 | 1. roslaunch lilee_gazebo edit_maze_pg.launch 61 | 2. Modify the world in gazebo 62 | 3. "Save As..." lilee_gazebo/worlds/maze_pg.world 63 | 64 | * color-follower (gazebo) 65 | 1. roslaunch lilee_gazebo turtlebot_maze.launch 66 | 2. rosrun lilee_tracker tracker.py 67 | 3. rosrun lilee_reinforcement agent 68 | 4. roslaunch lilee_follower follower.launch 69 | 5. rosrun lilee_reinforcement environment 70 | 71 | * color-follower (turtlebot) 72 | 1. roslaunch turtlebot_bringup minimal.launch 73 | 2. roslaunch lilee_follower follower.launch 74 | 3. rosrun lilee_tracker tracker.py 75 | 76 | ################################################################################ 77 | # Usage instructions # 78 | ################################################################################ 79 | 80 | * How to use the reinforcement packages: 81 | 1. Press Down arrow to turn user control ON/OFF. Default is OFF. 82 | 2. When user control is ON, use Up/Right/Left arrows to choose actions for 83 | the agent (turtlebot). 84 | 85 | * To close PG/PG-ELLA 86 | 1. ^C in terminal to kill lilee_gazebo and lilee_rl_continuous 87 | 2. Type `clear' in MATLAB 88 | 89 | ################################################################################ 90 | # Installation instructions # 91 | ################################################################################ 92 | 93 | * ROS I/O Package (MATLAB support for ROS): 94 | 1. Download ROS I/O Package from mathworks.com/ros 95 | 2. sudo apt-get install ia32-libs 96 | 3. Double-click ROSIOPackage-R2014a_v0.1.6.2_XXXXX-Install to launch the 97 | installer. 98 | 99 | * PG-ELLA 100 | - Policy Gradient Toolbox: ias.tu-darmstadt.de/Research/PolicyGradientToolbox 101 | - ELLA code: http://www.seas.upenn.edu/~eeaton/publications.html#Software 102 | 103 | ################################################################################ 104 | # Stored files # 105 | ################################################################################ 106 | 107 | * Q-Value stored files 108 | - The agent's Q-Value table is saved in ~/.ros/saved_qValue. 109 | - The agent's User Policy table is saved in ~/.ros/saved_userPolicy. 110 | 111 | 112 | --------------------------------------------------------------------------------