Poop topo nodes

[ibrahimhroob]

What type of PR is this? (check all applicable)

• Refactor
• Feature
• Bug Fix
• Optimization
• Documentation Update

Description

In this PR, I reintroduce the manual topo map creation tool originally developed by @sergimolina and migrate it to ROS2. Follow these steps to use the tool:

1. Launch the Tool:

   • Start by launching manual_topomapping.launch.py.

2. Add a New Node:

   • To add a new node, use the joystick and press the LT and A buttons simultaneously.
   • A new node will be added if the distance to the nearest node is greater than node_thresh. Refer to the figure below for button mapping.

3. Remove a Node:

   • To remove a node, press the LT and B buttons on the joystick.
   • This action will delete nodes within a 5-meter radius of the robot at one node at a time.

4. Save the Topo Map:

   • To save the generated topo map, press the LT and Y buttons on the joystick.
   • The map will be saved in the directory specified by tmap_dir, which can be configured in the launch file.

Please find the attached video below for illustrating how to use this tool.

Related Tickets & Documents

• Related Issue consolidate topological tools #183
• Closes #

QA Instructions, Screenshots, Recordings

Screencast.from.13-07-24.22.27.44.webm

[GPrathap]

Hi @ibrahimhroob , for the AOC project, we use the repository at https://github.com/GPrathap/topological_navigation.git (branch: humble-dev). Please send it there instead.

Thanks

[marc-hanheide]

Looks good in general. May I request changes to the README to explain what this does?

Also, is it possible to check that a waypoint does not exist yet (e.g. check if the new waypoint is within the verts of an existing one, and only create a new one if none exists? Then, can we make it so that the last waypoint where the button was pressed (could be an old or a new one) is used to create also an edge to the current one? Or is that too tricky?

[marc-hanheide]

Looks good in general. May I request changes to the README to explain what this does?

Also, is it possible to check that a waypoint does not exist yet (e.g. check if the new waypoint is within the verts of an existing one, and only create a new one if none exists? Then, can we make it so that the last waypoint where the button was pressed (could be an old or a new one) is used to create also an edge to the current one? Or is that too tricky?

Also, any idea what causes the assertion failure in https://github.com/LCAS/topological_navigation/actions/runs/9923089501/job/27412865755?pr=187#step:5:2365 ?

[ibrahimhroob]

Also, any idea what causes the assertion failure in https://github.com/LCAS/topological_navigation/actions/runs/9923089501/job/27412865755?pr=187#step:5:2365 ?

I am not sure as it did pass the humble distro, but is seems something related to nav2_msgs?

[Iranaphor]

Might it be worth including feature to either set the vert radii proportional to how far the back trigger is pressed in; or to have a few different options assigned to different buttons?

[ibrahimhroob]

Looks good in general. May I request changes to the README to explain what this does?

Also, is it possible to check that a waypoint does not exist yet (e.g. check if the new waypoint is within the verts of an existing one, and only create a new one if none exists? Then, can we make it so that the last waypoint where the button was pressed (could be an old or a new one) is used to create also an edge to the current one? Or is that too tricky?

Hi @marc-hanheide, yes I can do that. I'll add two options:

• A quick option is to create an edge to the previous node.
• Node selection via triggering the nodes from the closest to the furthest using a combination of buttons. As a visual aid, I'll try to color the selected node when cycling between them, with a button to confirm the selection.

[ibrahimhroob]

Might it be worth including feature to either set the vert radii proportional to how far the back trigger is pressed in; or to have a few different options assigned to different buttons?

Thanks for this suggestion, very nice idea, I will see what I can do ...

[marc-hanheide]

Hi @ibrahimhroob , for the AOC project, we use the repository at https://github.com/GPrathap/topological_navigation.git (branch: humble-dev). Please send it there instead.

Thanks

@GPrathap can we stop using personal forks please. Nothing against branches, I understand they are needed, but team members should work in the team's organisation so we can see and collaborate more easily. @ibrahimhroob is right that a PR should go against the upstream. Any fork can still pull from upstream then.

[ibrahimhroob]

I do have different proposal, instead of creating the edges during the session, I propose to create them as post process step, for that I will be using interactive markers at rviz where I can click two nodes to create edge, the order of clicking will determine the edge direction.

[ibrahimhroob]

Screencast.from.21-07-24.19.38.01.webm

In this illustration I am creating some random nodes using /goal_pose topic, then I use the interactive marker to select two nodes and create an edge between them. @marc-hanheide I think this maybe more handy to create the edges. What are your thoughts?

[ibrahimhroob]

Screencast.from.22-07-24.12.40.55.webm

I do have another proposal that is to create a simple GUI window that should the attribute of the selected node and edges and offer to modify them through the GUI. In the above video I am only should the clicked marker pose but it is easy to expand it for other tasks, like changing the name/pose/edge actions etc...

[marc-hanheide]

#187 (comment)

How does this relate to the existing RViz tools we have for toponav? Have you had a look at those existing ones? It feels like we’re reinventing what we used to have? Just checking if you had reviewed what’s there.

[marc-hanheide]

also, can you figure out why the CI is failing? Is it an outdated test, or a race condition?

[Iranaphor]

The existing rviz tools were fairly over-engineered for what they achieved, I think this approach works much better than they did. If a standard collection of topics are used (and they just happen to be defaulted to the same topics as the default rviz tools), it could be built into an API for standardised dashboard interfaces.

[marc-hanheide]

I agree they were “over-engineered”, but I think we still need a “coherent” toolset.

[ibrahimhroob]

#187 (comment)

How does this relate to the existing RViz tools we have for toponav? Have you had a look at those existing ones? It feels like we’re reinventing what we used to have? Just checking if you had reviewed what’s there.

I just had a look at: https://github.com/LCAS/topological_navigation/tree/humble-dev/topological_rviz_tools, but it seems all still in ROS1? and has not be migrated to ROS2 yet. But I do agree all of those tools need to be in one place.

[ibrahimhroob]

I think for this manual topo map I will keep it as simple as possible and just add the edge creation tool by clicking on two nodes with no GUI or other complicated things (which will use the stander API of RVIZ2).

[ibrahimhroob]

also, can you figure out why the CI is failing? Is it an outdated test, or a race condition?

Please correct me if I'm mistaken, but it appears the failure occurred here, which corresponds to this test:

topological_navigation/topological_navigation/test/test_navigationcore.py

Lines 78 to 86 in 6206e9f

	def test_check_if_current_node_is_published():
	rclpy.init()
	try:
	node = NavigationClient('test_node')
	node.initialize()
	msgs_received_flag = node.current_node_event_object.wait(timeout=20.0)
	assert msgs_received_flag, 'Did not receive current node info !'
	finally:
	rclpy.shutdown()

.

However, when I ran it again, the test passed successfully. I also ran the tests locally on my machine, and they all passed. Could this be a "race condition"? If so, would increasing the test timeout resolve the issue?

When the test fails it took 25 seconds:

However in the successful run it took 8 seconds.

While on my machine it took 6 seconds:

So I believe this is timeout issue with the test.

[marc-hanheide]

Looks like a race condition… We need to check why the tests keep failing.

[marc-hanheide]

Ping

[ibrahimhroob]

I did increase the test timeout and it solved it