├── 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 |
--------------------------------------------------------------------------------