New demo to test action aborting feature

demos/server_abort_goals/src/server_abort_goals.app.src

@@ -0,0 +1,15 @@
+{application, server_abort_goals,
+ [{description, "Example action server"},
+  {vsn, "0.1.0"},
+  {registered, []},
+  {mod, {server_abort_goals_app, []}},
+  {applications,
+   [kernel,
+    stdlib,
+    ros
+   ]},
+  {env,[]},
+  {modules, []},
+  {licenses, ["Apache 2.0"]},
+  {links, []}
+ ]}.

demos/server_abort_goals/src/server_abort_goals.erl

@@ -0,0 +1,51 @@
+-module(server_abort_goals).
+-export([start_link/0]).
+
+-behaviour(gen_action_server_listener).
+-export([on_execute_goal/2]).
+
+-behaviour(gen_server).
+-export([init/1, handle_call/3, handle_cast/2, handle_info/2]).
+
+-include_lib("example_interfaces/src/_rosie/example_interfaces_fibonacci_action.hrl").
+
+-record(state, {ros_node, action_server}).
+
+-define(LOCAL_SRV, action_client).
+
+start_link() ->
+    gen_server:start_link({local, ?LOCAL_SRV}, ?MODULE, [], []).
+
+% callbacks for gen_action_server_listener
+on_execute_goal(Pid, Goal) ->
+    gen_server:cast(Pid, {on_execute_goal, Goal}).
+
+
+% callbacks for gen_server
+init(_) ->
+    Node = ros_context:create_node("minimal_action_server"),
+
+    ActionServer = ros_context:create_action_server(Node,
+                                         example_interfaces_fibonacci_action,
+                                         {?MODULE, self()}),
+
+    {ok, #state{ros_node = Node, action_server = ActionServer}}.
+
+handle_call(_, _, S) ->
+    {reply, ok, S}.
+
+handle_cast({on_execute_goal,#example_interfaces_fibonacci_send_goal_rq{goal_id = UUID, order = ORDER}}, S) ->
+    io:format("Executing goal: ~p\n", [UUID#unique_identifier_msgs_u_u_i_d.uuid]),
+    erlang:send_after(3000, self(), {next_step, UUID, [1, 0], ORDER - 2}),
+    {noreply, S}.
+
+
+handle_info({next_step, UUID,[Last_1, Last_2 | _] = L, _},
+            #state{action_server = AS} = S) ->
+    Step = [Last_1 + Last_2 | L],
+    List = lists:reverse(Step),
+    io:format("Publishing feedback: ~p\n", [List]),
+    ros_action_server:publish_feedback(AS, #example_interfaces_fibonacci_feedback_message{goal_id = UUID, sequence = List}),
+    io:format("Aborting goal: ~p\n", [UUID#unique_identifier_msgs_u_u_i_d.uuid]),
+    ros_action_server:abort_goal(AS, UUID),
+    {noreply, S}.

demos/server_abort_goals/src/server_abort_goals_app.erl

@@ -0,0 +1,13 @@
+-module(server_abort_goals_app).
+
+-behaviour(application).
+
+-export([start/2, stop/1]).
+
+start(_StartType, _StartArgs) ->
+    server_abort_goals_sup:start_link().
+
+stop(_State) ->
+    ok.
+
+%% internal functions

demos/server_abort_goals/src/server_abort_goals_sup.erl

@@ -0,0 +1,39 @@
+-module(server_abort_goals_sup).
+
+-behaviour(supervisor).
+
+-export([start_link/0]).
+-export([init/1]).
+
+-define(SERVER, ?MODULE).
+
+start_link() ->
+    supervisor:start_link({local, ?SERVER}, ?MODULE, []).
+
+%% sup_flags() = #{strategy => strategy(),         % optional
+%%                 intensity => non_neg_integer(), % optional
+%%                 period => pos_integer()}        % optional
+%% child_spec() = #{id => child_id(),       % mandatory
+%%                  start => mfargs(),      % mandatory
+%%                  restart => restart(),   % optional
+%%                  shutdown => shutdown(), % optional
+%%                  type => worker(),       % optional
+%%                  modules => modules()}   % optional
+
+init([]) ->
+    SupFlags =
+        #{strategy => one_for_all,
+          intensity => 0,
+          period => 1},
+    Server =
+        #{id => server_abort_goals,
+          start => {server_abort_goals, start_link, []},
+          restart => transient,
+          shutdown => 5000,
+          type => worker},
+
+    ChildSpecs = [Server],
+
+    {ok, {SupFlags, ChildSpecs}}.
+
+%% internal functions

rosie/ros/src/ros_action_server.erl

@@ -2,7 +2,7 @@
 -export([start_link/3, destroy/1]).
 
 % API
--export([cancel_goal/2, publish_feedback/2, publish_result/3]).
+-export([abort_goal/2, cancel_goal/2, publish_feedback/2, publish_result/3]).
 
 -behaviour(gen_service_listener).
 
@@ -45,6 +45,10 @@ destroy(Name) ->
     [Pid | _] = pg:get_members(Name),
     gen_server:stop(Pid).
 
+abort_goal(Name, Msg) ->
+    [Pid | _] = pg:get_members(Name),
+    gen_server:cast(Pid, {abort_goal, Msg}).
+
 cancel_goal(Name, Msg) ->
     [Pid | _] = pg:get_members(Name),
     gen_server:cast(Pid, {cancel_goal, Msg}).
@@ -126,13 +130,16 @@ handle_call({publish_result, GoalID, Result},
                    get_result_service = GetResultService,
                    cached_goal_results = Cached} =
                 S) ->
+    % Always cache the result
+    NewState = S#state{cached_goal_results = [Result | Cached]},
     case [{ClientID, RN} || {ClientID, RN, ID, _} <- GRR, ID == GoalID] of
         [] ->
-            {reply, ok, S#state{cached_goal_results = [Result | Cached]}};
+            {reply, ok, NewState};
         RRL ->
-            [ros_service:send_response(GetResultService, {ClientID, RN, Result})
-             || {ClientID, RN} <- RRL],
-            {reply, ok, publish_goal_status_update(mark_goal_as(GoalID, ?STATUS_SUCCEEDED, S))}
+            [ros_service:send_response(GetResultService, {ClientID, RN, Result}) || {ClientID, RN} <- RRL],
+            NewState = mark_goal_as(GoalID, ?STATUS_SUCCEEDED, NewState),
+            publish_goal_status_update(NewState),
+            {reply, ok, NewState}
     end;
 handle_call({on_client_request, {{ClientId, RequestNumber}, Msg}},
             _,
@@ -149,27 +156,42 @@ handle_call({on_client_request, {{ClientId, RequestNumber}, Msg}},
             {reply, error, S}
     end.
 
+handle_cast({abort_goal, UUID},
+            #state{action_interface = AI, cancel_goal_service = CancelGoalService, goals_accepted = GA} = S) ->
+    case (maps:get(UUID, GA))#goal.status of
+        STATUS when (STATUS == ?STATUS_EXECUTING) or (STATUS == ?STATUS_CANCELING) ->
+            NewState = mark_goal_as(UUID, ?STATUS_ABORTED, S),
+            NewState1 = close_pending_goal_requests(UUID, NewState),
+            publish_goal_status_update(NewState1),
+            {noreply, NewState1};
+        _ -> 
+            {noreply, S}
+    end;
 handle_cast({cancel_goal, UUID},
-            #state{action_interface = AI, cancel_goal_service = CancelGoalService} = S) ->
-    G_INFO = #action_msgs_goal_info{goal_id = UUID},
-    NewState = clear_cache_for_goal(UUID, mark_goal_as(UUID, ?STATUS_CANCELED, S)),
-    {noreply, publish_goal_status_update(NewState)};
+            #state{action_interface = AI, cancel_goal_service = CancelGoalService, goals_accepted = GA} = S) ->
+    case (maps:get(UUID, GA))#goal.status of
+        ?STATUS_CANCELING ->
+            NewState = mark_goal_as(UUID, ?STATUS_CANCELED, S),
+            NewState1 = close_pending_goal_requests(UUID, NewState),
+            publish_goal_status_update(NewState1),   
+            {noreply, NewState1};
+        _ -> 
+            {noreply, S}
+    end;
 handle_cast(_, S) ->
     {noreply, S}.
 
 publish_goal_status_update(#state{action_interface = AI,
                                   status_publisher = S_PUB,
-                                  goals_accepted = GA} =
-                               S) ->
+                                  goals_accepted = GA}) ->
     LIST =
         [#action_msgs_goal_status{goal_info = #action_msgs_goal_info{goal_id = UUID, stamp = T},
                                   status = STATUS}
          || #goal{uuid = UUID,
                   time = T,
                   status = STATUS}
                 <- maps:values(GA)],
-    ros_publisher:publish(S_PUB, #action_msgs_goal_status_array{status_list = LIST}),
-    S.
+    ros_publisher:publish(S_PUB, #action_msgs_goal_status_array{status_list = LIST}).
 
 h_manage_goal_request(Msg,
                       #state{action_interface = AI,
@@ -178,7 +200,7 @@ h_manage_goal_request(Msg,
                           S) ->
     Reply = case erlang:function_exported(M, on_new_goal_request, 2) of
         true -> M:on_new_goal_request(Pid, Msg);
-        false -> AI:accept_goal_reply() % reject as default
+        false -> AI:accept_goal_reply() % accept as default
     end,
     case AI:get_responce_code(Reply) of
         0 ->
@@ -191,13 +213,20 @@ h_manage_goal_request(Msg,
                                     #goal{uuid = AI:get_goal_id(Msg),
                                           time = #builtin_interfaces_time{},
                                           status = ?STATUS_EXECUTING}}},
-            {reply, Reply, publish_goal_status_update(NewState)}
+            publish_goal_status_update(NewState),
+            {reply, Reply, NewState}
     end.
 
 clear_cache_for_goal(UUID,
                      #state{action_interface = AI, cached_goal_results = CachedResults} = S) ->
     S#state{cached_goal_results = [R || R <- CachedResults, AI:get_goal_id(R) /= UUID]}.
 
+close_pending_goal_requests(UUID,#state{get_result_service = RS, action_interface = AI, goals_accepted = GA, goals_with_requested_results = GRR} = S) ->
+    GoalState = (maps:get(UUID, GA))#goal.status,
+    {Matching, Others} = lists:partition(fun({_, _, GoalID, _}) -> GoalID == UUID end, GRR),
+    [ros_service:send_response(RS, {ClientID, RN, AI:failed_result_reply(GoalState)}) || {ClientID, RN, _, _} <- Matching],
+    S#state{goals_with_requested_results = Others}.
+
 mark_goal_as(UUID, NewGoalState, #state{goals_accepted = GA} = S) ->
     case maps:get(UUID, GA, not_found) of
         not_found ->
@@ -231,7 +260,8 @@ h_manage_result_request(ClientID,
                                   | GRQ]}};
                 [R | _] ->
                     NewS = mark_goal_as(AI:get_goal_id(R), ?STATUS_SUCCEEDED, S),
-                    {reply, R, publish_goal_status_update(NewS)}
+                    publish_goal_status_update(NewS),
+                    {reply, R, NewS}
             end
     end.
 
@@ -257,7 +287,7 @@ h_manage_cancel_request(#action_msgs_cancel_goal_rq{goal_info =
     case {GOAL, Has_callback} of
         {not_found, _} -> 
             {reply, #action_msgs_cancel_goal_rp{return_code = ?ERROR_UNKNOWN_GOAL_ID}, S};
-        {#goal{status = ?STATUS_CANCELED}, _}->
+        {#goal{status = ?STATUS_CANCELED}, _} ->
             {reply, #action_msgs_cancel_goal_rp{return_code = ?ERROR_GOAL_TERMINATED}, S};
         {#goal{status = ?STATUS_SUCCEEDED}, _} ->
             {reply, #action_msgs_cancel_goal_rp{return_code = ?ERROR_GOAL_TERMINATED}, S};
@@ -268,10 +298,9 @@ h_manage_cancel_request(#action_msgs_cancel_goal_rq{goal_info =
                 accept ->
                     G_INFO = #action_msgs_goal_info{goal_id = UUID},
                     M:on_cancel_goal(Pid, UUID),
-                    {reply,
-                     #action_msgs_cancel_goal_rp{return_code = ?ERROR_NONE,
-                                                 goals_canceling = [G_INFO]},
-                     publish_goal_status_update(mark_goal_as(UUID, ?STATUS_CANCELING, S))};
+                    NewS = mark_goal_as(UUID, ?STATUS_CANCELING, S),
+                    publish_goal_status_update(NewS),
+                    {reply, #action_msgs_cancel_goal_rp{return_code = ?ERROR_NONE, goals_canceling = [G_INFO]}, NewS};
                 reject ->
                     {reply, #action_msgs_cancel_goal_rp{return_code = ?ERROR_REJECTED}, S};
                 _ ->