Added tmap2 support for navigation.py and execute_policy

[ayu135]

This adds the compatibility to tmap2 for navigate.py and execute_policy_server.py and also merges the actions serves for both in a single node.

• Updated navigate.py to support tmap2
• Updated execute_policy_server to use tmap2
• Now both navigate and execute policy action servers run from the same node
• Updated some functions in tmap utils to support tmap2

Still pending all testing, which i will try to complete soon

[adambinch]

How do you tell navigation to use tmap2? It looks like it is hard coded as False?
If i set that arg (use_tmap2) to True in navigation.py i get this:

Traceback (most recent call last):
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/adam/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 253, in MapCallback
    self.topol_map = json.loads(msg.data["pointset"])
TypeError: string indices must be integers, not str

[adambinch]

I think https://github.com/ayu135/topological_navigation/blob/febc6c227a9d5beddd0d94fb7c93ae933b2f94f6/topological_navigation/scripts/navigation.py#L253

should be

self.topol_map = json.loads(msg.data)["pointset"]

or better still

self.topol_map = self.lnodes["pointset"]

[ayu135]

How do you tell navigation to use tmap2? It looks like it is hard coded as False?
If i set that arg (use_tmap2) to True in navigation.py i get this:

Traceback (most recent call last):
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/adam/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 253, in MapCallback
    self.topol_map = json.loads(msg.data["pointset"])
TypeError: string indices must be integers, not str

Yes, sorry I had intended to have a discussion about that at some point but it slipped my mind. I wasn't sure if it is a better idea to have that as a launch param or a command line arg like you have done in localization.py. I will add it as an arg for now like the other script.

[adambinch]

Getting another error when running navigation with tmap2

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/adam/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 271, in executeCallback
    self.navigate(goal.target)
  File "/home/adam/rasberry_ws/src/topological_navigation/topological_navigation/scripts/navigation.py", line 350, in navigate
    and (g_node.name != o_node.name)
AttributeError: 'dict' object has no attribute 'name'

[adambinch]

So we need the edge id field to run the navigation and test this PR properly. The edge id field is included in #44. So perhaps that should be merged first @Jailander ?

[ayu135]

Yeah and also we would need to modify the search_route.py to support tmap2 as well for this to work correctly

[adambinch]

This Works! :)