Combine execute policy and nav actions in a single script

[ayu135]

This removes the execute_policy_server.py file and moves the execute policy action server to the navigation.py script. That script launches both the action servers now.

I have tested this with single robot scenarios for both execute policy and nav actions using toponav2-devel and rasberry polytunnel scenarios.

I am setting up multi sim and havent tested with that yet.

I would be glad if @gpdas and @adambinch can take a look and test this as well

[ayu135]

As per my discussion with @Jailander and @adambinch I have reworked the navigation.py script. It now has the following:

• Common dynparam reconfigure for both which is based on the way it was done in navigation script earlier
• Common follow_route function which is called from each action callback when a new goal is received, this replaces some of the follow_route logic for exec policy and makes it more consistent with nav and removes redundancy by having separate functions for both
• Basically exec policy logic is more simpler and consistent with nav action now

[gpdas]

I was trying to call both actions at the same time to see what happens.

1. What is the expected behaviour when we call one action while the other is active? I tried it in one direction (calling toponav when exec_policy is active). robot planned the route for toponav, but continued executing the exec_policy.
2. I also got this error.

   navigation.py", line 736, in followRoute_tmap2
   self.stat.set_ended(self.current_node)
   AttributeError: 'NoneType' object has no attribute 'set_ended'

Also as a side note, one could use the rasberry_multisim.yaml from toponav2-devel branch of RASberry from my fork for testing.

[adambinch]

Seen that attribute error myself

[ayu135]

@gpdas @adambinch Can you guys help me reproduce the attribute error mentioned above? When is that happening?

Also for the behaviour when the other action is called when one is executing, it should preempt the other goal and start with the new one. I will test that out a bit more.

[adambinch]

Strange I have been testing this with both action servers for a while now and I am not getting that attribute error

[francescodelduchetto]

I tested with rasberry_multisim.yaml from @gpdas 's branch and I could not reproduce the attribute error as well. However,

• I get this error every time a navigation goal is preempted by the other (either toponav preempted by execpolicy or reverse):

  
  [INFO] [1616149148.819073, 2045.340000]: Concurrence serviced preempt.
  [INFO] [1616149148.820647, 2045.340000]: Concurrent Outcomes: {'NAV_SM': 'preempted'}
  [INFO] [1616149148.821596, 2045.340000]: State machine terminating 'MONITORED_NAV':'preempted':'preempted'
  [INFO] [1616149148.995968, 2045.520000]: Fatal fail on Friday, March 19 2021, at 10:19:08 hours (1/0)
  [ERROR] [1616149148.996185, 2045.520000]: SimpleActionServer.internal_goal_callback - exception service call failed
  [ERROR] [1616149149.000000, 2045.525000]: Exception in your execute callback: service call failed
  Traceback (most recent call last):
  File "/opt/ros/melodic/lib/python2.7/dist-packages/actionlib/simple_action_server.py", line 289, in executeLoop
  self.execute_callback(goal)
  File "/home/francesco/workspaces/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 383, in executeCallbackexecpolicy
  result = self.execute_policy(goal.route)
  File "/home/francesco/workspaces/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 414, in execute_policy
  succeeded, inc = self.followRoute_tmap2(route, target, 1)
  File "/home/francesco/workspaces/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 727, in followRoute_tmap2
  self.reconfigure_movebase_params(params)
  File "/home/francesco/workspaces/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 243, in reconfigure_movebase_params
  self._do_movebase_reconf(translated_params)
  File "/home/francesco/workspaces/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 247, in _do_movebase_reconf
  self.rcnfclient.update_configuration(params)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/dynamic_reconfigure/client.py", line 224, in update_configuration
  raise DynamicReconfigureCallbackException('service call failed')
  DynamicReconfigureCallbackException: service call failed

[INFO] [1616149149.014484, 2045.535000]: Navigating Case 1 -> res: 1 [INFO] [1616149149.017178, 2045.540000]: Navigating next try: 1 [INFO] [1616149149.106060, 2045.630000]: Creating Reconfigure Client

- the robots always end up stuck in a situation like this while traversing the rows, this may be an in-row navigation problem though

![Screenshot from 2021-03-19 10-30-01](https://user-images.githubusercontent.com/7307164/111767004-198fd900-889e-11eb-970c-c4425b20ad6b.png)

[adambinch]

@ayu135 you mentioned in out google chats that you had a suspicion that the attribute error occurs when sending invalid routes. So tried sending dodgy routes with the execute_policy_mode. Nope still not getting the error.

Maybe we could just do this:

if self.stat is not None:
    self.stat.set_ended(self.current_node)

haha

[francescodelduchetto]

@adambinch @Jailander @ayu135 these commits in https://github.com/ayu135/topological_navigation/pull/2 should solve the errors above, please have a look.

It works for me on the transportation multisim map, ~I still have to check this on the UV map to see if the robots still stop navigating after entering the tunnels as above~ on UV I was just using a short robot which was colliding with the poles.

[ayu135]

I'll test is, thanks a lot! if that works i guess this is ready to be merged. I will test it today and report back

[adambinch]

Works fine with https://github.com/ayu135/topological_navigation/pull/2. Goal pre-empting is fixed :) @ayu135 if it works for you can you merge that PR into your branch, then merge this one?

[ayu135]

Yeah it is working fine for me too! Done with my testing and everything seems to be ok now. I am merging this to PR as well