Skip to content

Commit 91cc3a2

Browse files
MidnightRaider06mdurrani808Rishav-N
authored
Feature/waypoints nav (#111)
* Waypoint package draft * Working version of waypoints * Added msg, srv, and actions for waypoints * Added waypoint manager to navigation launch * Removed altitude, added new service for manually adding waypoints * Added functionality for saving/loading waypoints to/from a file for persistence --------- Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com> Co-authored-by: Rishav-N <78056635+Rishav-N@users.noreply.github.com>
1 parent bbf7571 commit 91cc3a2

14 files changed

Lines changed: 513 additions & 15 deletions

File tree

‎src/msgs/CMakeLists.txt‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,16 @@ rosidl_generate_interfaces(${PROJECT_NAME}
2020
"msg/JointStatus.msg"
2121
"msg/SystemInfo.msg"
2222
"msg/Heading.msg"
23-
"msg/LedStatus.msg"
23+
"msg/WaypointEntry.msg"
24+
"msg/WaypointManagerState.msg"
2425
"srv/SetController.srv"
26+
"srv/AddWaypoint.srv"
27+
"srv/ClearWaypoints.srv"
28+
"msg/LedStatus.msg"
2529
"srv/StatusReq.srv"
2630
"srv/MaintenanceReq.srv"
2731
"action/NavigateToGPS.action"
32+
"action/NavigateToWaypoint.action"
2833
DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs
2934
)
3035

‎src/msgs/action/NavigateToWaypoint.action‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
uint32 waypoint_id
2+
---
3+
bool success
4+
string message
5+
---
6+
string status
7+
float32 distance_remaining

‎src/msgs/msg/WaypointEntry.msg‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
uint32 id
2+
float64 latitude
3+
float64 longitude
4+
builtin_interfaces/Time timestamp

‎src/msgs/msg/WaypointManagerState.msg‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
uint8 STATUS_IDLE = 0
2+
uint8 STATUS_NAVIGATING = 1
3+
uint8 STATUS_SUCCEEDED = 2
4+
uint8 STATUS_CANCELED = 3
5+
uint8 STATUS_FAILED = 4
6+
7+
uint32 active_waypoint_id
8+
uint8 nav_status
9+
WaypointEntry[] waypoints
10+
builtin_interfaces/Time last_update

‎src/msgs/srv/AddWaypoint.srv‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
float64 latitude
2+
float64 longitude
3+
---
4+
bool success
5+
string message
6+
uint32 assigned_id

‎src/msgs/srv/ClearWaypoints.srv‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
---
2+
bool success
3+
string message

‎src/subsystems/navigation/athena_planner/launch/navigation.launch.py‎

Lines changed: 23 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -21,14 +21,17 @@ def generate_launch_description():
2121
localizer_launch_file = os.path.join(localizer_share, 'launch', 'localizer.launch.py')
2222
zed_tf_publisher_launch_file = os.path.join(localizer_share, 'launch', 'zed_tf_publisher.launch.py')
2323

24-
# gps_goal_share = get_package_share_directory('gps_goal')
25-
# gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py')
24+
gps_goal_share = get_package_share_directory('gps_goal')
25+
gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py')
2626

2727
sensors_share = get_package_share_directory('athena_sensors')
2828
sensors_launch_file = os.path.join(sensors_share, 'launch', 'sensors.launch.py')
2929

30-
# aruco_bt_share = get_package_share_directory('aruco_bt')
31-
# aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py')
30+
aruco_bt_share = get_package_share_directory('aruco_bt')
31+
aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py')
32+
33+
waypoint_share = get_package_share_directory('waypoint_manager')
34+
waypoint_launch_file = os.path.join(waypoint_share, 'launch', 'waypoint_manager.launch.py')
3235

3336
default_params = PathJoinSubstitution([
3437
FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml'
@@ -91,15 +94,20 @@ def generate_launch_description():
9194
}.items(),
9295
)
9396

94-
# aruco_launch = IncludeLaunchDescription(
95-
# PythonLaunchDescriptionSource(aruco_launch_file),
96-
# launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items()
97-
# )
97+
aruco_launch = IncludeLaunchDescription(
98+
PythonLaunchDescriptionSource(aruco_launch_file),
99+
launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items()
100+
)
98101

99-
# gps_goal_launch = IncludeLaunchDescription(
100-
# PythonLaunchDescriptionSource(gps_goal_launch_file),
101-
# launch_arguments={'use_sim_time': sim}.items(),
102-
# )
102+
waypoint_launch = IncludeLaunchDescription(
103+
PythonLaunchDescriptionSource(waypoint_launch_file),
104+
launch_arguments={'use_sim_time': sim}.items()
105+
)
106+
107+
gps_goal_launch = IncludeLaunchDescription(
108+
PythonLaunchDescriptionSource(gps_goal_launch_file),
109+
launch_arguments={'use_sim_time': sim}.items(),
110+
)
103111

104112
point_cloud_filterer_sim = Node(
105113
package='point_cloud_filterer',
@@ -170,10 +178,11 @@ def generate_launch_description():
170178
localizer_launch,
171179
zed_tf_publisher_launch,
172180
sensors_launch,
173-
# aruco_launch,
181+
aruco_launch,
182+
waypoint_launch,
174183
point_cloud_filterer_sim,
175184
point_cloud_relay,
176-
# gps_goal_launch,
185+
gps_goal_launch,
177186

178187
IncludeLaunchDescription(
179188
PythonLaunchDescriptionSource(nav2_nav),

‎src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
from launch import LaunchDescription
2+
from launch.actions import DeclareLaunchArgument
3+
from launch.substitutions import LaunchConfiguration
4+
from launch_ros.actions import Node
5+
6+
7+
def generate_launch_description():
8+
return LaunchDescription([
9+
DeclareLaunchArgument('use_sim_time', default_value='false'),
10+
DeclareLaunchArgument(
11+
'waypoints_file',
12+
default_value='~/.ros/waypoints.yaml',
13+
description='Path to YAML file for waypoint persistence'
14+
),
15+
DeclareLaunchArgument(
16+
'load_from_file',
17+
default_value='true',
18+
description='Load saved waypoints from file on startup'
19+
),
20+
Node(
21+
package='waypoint_manager',
22+
executable='waypoint_manager_node',
23+
name='waypoint_manager',
24+
parameters=[{
25+
'use_sim_time': LaunchConfiguration('use_sim_time'),
26+
'waypoints_file': LaunchConfiguration('waypoints_file'),
27+
'load_from_file': LaunchConfiguration('load_from_file'),
28+
}],
29+
output='screen',
30+
),
31+
])

‎src/subsystems/navigation/waypoint_manager/package.xml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>waypoint_manager</name>
5+
<version>0.0.0</version>
6+
<description>Waypoint management and navigation node for simulation</description>
7+
<maintainer email="abhinav.kota06@gmail.com">abhinavkota06</maintainer>
8+
<license>MIT</license>
9+
10+
<depend>rclpy</depend>
11+
<depend>msgs</depend>
12+
<depend>nav2_msgs</depend>
13+
<depend>geometry_msgs</depend>
14+
<depend>std_msgs</depend>
15+
<depend>action_msgs</depend>
16+
<depend>tf2_ros</depend>
17+
<depend>ament_index_python</depend>
18+
<depend>python3-yaml</depend>
19+
20+
<test_depend>ament_copyright</test_depend>
21+
<test_depend>ament_flake8</test_depend>
22+
<test_depend>ament_pep257</test_depend>
23+
<test_depend>python3-pytest</test_depend>
24+
25+
<export>
26+
<build_type>ament_python</build_type>
27+
</export>
28+
</package>

‎src/subsystems/navigation/waypoint_manager/resource/waypoint_manager‎

Whitespace-only changes.

0 commit comments

Comments
 (0)