Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Poop topo nodes #187

Open
wants to merge 6 commits into
base: humble-dev
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions topological_navigation/config/template_edge.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
action: move_base
action_type: move_base_msgs/MoveBaseGoal
config: []
edge_id: origin_destination
fail_policy: fail
fluid_navigation: True
goal:
target_pose:
header:
frame_id: $node.parent_frame
pose: $node.pose
node: destination
recovery_behaviours_config: ''
restrictions_planning: 'True'
restrictions_runtime: 'True'
41 changes: 41 additions & 0 deletions topological_navigation/config/template_node_2.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
meta:
map: map_2d
node: NodeName
pointset: PointSet
node:
edges: []
localise_by_topic: ''
name: NodeName
parent_frame: map
pose:
orientation:
w: 1.0
x: 0.0
y: 0.0
z: 0.0
position:
x: 0.0
y: 0.0
z: 0.0
properties:
xy_goal_tolerance: 0.3
yaw_goal_tolerance: 6.29
restrictions_planning: 'True'
restrictions_runtime: 'True'
verts:
- x: 0.689999997616
y: 0.287000000477
- x: 0.287000000477
y: 0.490000009537
- x: -0.287000000477
y: 0.490000009537
- x: -0.689999997616
y: 0.287000000477
- x: -0.689999997616
y: -0.287000000477
- x: -0.287000000477
y: -0.490000009537
- x: 0.287000000477
y: -0.490000009537
- x: 0.689999997616
y: -0.287000000477
42 changes: 42 additions & 0 deletions topological_navigation/launch/manual_topomapping.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
'''
Author: Ibrahim Hroob <ihroob@lincoln.ac.uk> 2024
'''
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
return LaunchDescription([
# Declare the launch arguments
DeclareLaunchArgument('tmap', default_value='test.yaml'),
DeclareLaunchArgument('tmap_dir', default_value='/home/ros/tmap'),
DeclareLaunchArgument('node_thresh', default_value='0.5'),
DeclareLaunchArgument('lock_btn', default_value='6'),
DeclareLaunchArgument('add_btn', default_value='1'),
DeclareLaunchArgument('remove_btn', default_value='2'),
DeclareLaunchArgument('gen_map_btn', default_value='3'),
DeclareLaunchArgument('topic_joy', default_value='/joy'),
DeclareLaunchArgument('topic_pose', default_value='/gps_base/odometry'),

# Launch the manual_topomapping node
Node(
package='topological_navigation',
executable='manual_topomapping.py',
name='manual_topomapping',
output='screen',
parameters=[{
'tmap' : LaunchConfiguration('tmap'),
'tmap_dir' : LaunchConfiguration('tmap_dir'),
'site_name' : LaunchConfiguration('tmap'),
'node_thresh': LaunchConfiguration('node_thresh'),
'lock_btn' : LaunchConfiguration('lock_btn'),
'add_btn' : LaunchConfiguration('add_btn'),
'remove_btn' : LaunchConfiguration('remove_btn'),
'gen_map_btn': LaunchConfiguration('gen_map_btn'),
'topic_joy' : LaunchConfiguration('topic_joy'),
'topic_pose' : LaunchConfiguration('topic_pose'),
}]
),
])
3 changes: 2 additions & 1 deletion topological_navigation/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@
'visualise_map_ros2.py = topological_navigation.scripts.visualise_map_ros2:main',
'topomap_marker.py = topological_navigation.topomap_marker:main',
'topomap_marker2.py = topological_navigation.topomap_marker2:main',
'policy_marker.py = topological_navigation.policy_marker:main'
'policy_marker.py = topological_navigation.policy_marker:main',
'manual_topomapping.py = topological_navigation.scripts.manual_topomapping:main'
],
},

Expand Down
6 changes: 3 additions & 3 deletions topological_navigation/test/test_navigationcore.py
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ def test_check_if_topological_map_is_published():
try:
node = NavigationClient('test_node')
node.initialize()
msgs_received_flag = node.topmap_event_object.wait(timeout=20.0)
msgs_received_flag = node.topmap_event_object.wait(timeout=40.0)
assert msgs_received_flag, 'Did not receive topological map !'
finally:
rclpy.shutdown()
Expand All @@ -69,7 +69,7 @@ def test_check_if_closest_node_is_published():
try:
node = NavigationClient('test_node')
node.initialize()
msgs_received_flag = node.closest_node_event_object.wait(timeout=20.0)
msgs_received_flag = node.closest_node_event_object.wait(timeout=40.0)
assert msgs_received_flag, 'Did not receive closest node info !'
finally:
rclpy.shutdown()
Expand All @@ -80,7 +80,7 @@ def test_check_if_current_node_is_published():
try:
node = NavigationClient('test_node')
node.initialize()
msgs_received_flag = node.current_node_event_object.wait(timeout=20.0)
msgs_received_flag = node.current_node_event_object.wait(timeout=40.0)
assert msgs_received_flag, 'Did not receive current node info !'
finally:
rclpy.shutdown()
Expand Down
Loading
Loading