Skip to content

Commit ac9be26

Browse files
zhangwanjiezhangwanjie
committed
add navi_waypoint
1 parent d82513a commit ac9be26

8 files changed

+982
-7
lines changed

‎CMakeLists.txt

+16-2
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,11 @@ endif()
88
# find dependencies
99
find_package(ament_cmake REQUIRED)
1010
find_package(geometry_msgs REQUIRED)
11+
find_package(std_msgs REQUIRED)
1112
find_package(visualization_msgs REQUIRED)
1213
find_package(interactive_markers REQUIRED)
13-
find_package(nav_msgs REQUIRED)
14+
find_package(nav2_msgs REQUIRED)
15+
find_package(rclcpp_action REQUIRED)
1416
find_package(rclcpp REQUIRED)
1517
find_package(rclcpp_components REQUIRED)
1618
find_package(yaml-cpp REQUIRED)
@@ -40,12 +42,14 @@ ament_export_dependencies(
4042
"tf2_ros"
4143
"visualization_msgs"
4244
"geometry_msgs"
45+
"std_msgs"
4346
"yaml-cpp")
4447

4548
find_package(rosidl_default_generators REQUIRED)
4649
rosidl_generate_interfaces(${PROJECT_NAME}
4750
"msg/Waypoint.msg"
4851
"srv/SaveWaypoints.srv"
52+
"srv/GetWaypointByName.srv"
4953
DEPENDENCIES
5054
"geometry_msgs"
5155
)
@@ -142,9 +146,17 @@ ament_target_dependencies(wp_edit_node
142146
target_link_libraries(wp_edit_node "yaml-cpp" "${cpp_typesupport_target}")
143147

144148
add_executable(wp_saver src/wp_saver.cpp)
145-
ament_target_dependencies(wp_saver rclcpp )
149+
ament_target_dependencies(wp_saver "rclcpp" )
146150
target_link_libraries(wp_saver "${cpp_typesupport_target}")
147151

152+
add_executable(wp_navi_server src/wp_navi_server.cpp)
153+
ament_target_dependencies(wp_navi_server "rclcpp" "std_msgs" "rclcpp_action" "nav2_msgs" "tf2_ros")
154+
target_link_libraries(wp_navi_server "${cpp_typesupport_target}")
155+
156+
add_executable(demo_navi_waypoint src/demo_navi_waypoint.cpp)
157+
ament_target_dependencies(demo_navi_waypoint "rclcpp" "std_msgs" )
158+
target_link_libraries(demo_navi_waypoint "${cpp_typesupport_target}")
159+
148160
# 安装节点
149161
install(DIRECTORY launch meshes rviz msg srv icons
150162
DESTINATION share/${PROJECT_NAME})
@@ -153,6 +165,8 @@ install(
153165
TARGETS
154166
wp_edit_node
155167
wp_saver
168+
wp_navi_server
169+
demo_navi_waypoint
156170
DESTINATION
157171
lib/${PROJECT_NAME}
158172
)

‎launch/marker_navigation.launch.py

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright 2023 6-robot.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
#
17+
# Authors: Zhang Wanjie
18+
19+
import os
20+
from ament_index_python.packages import get_package_share_directory
21+
from launch import LaunchDescription
22+
from launch.actions import IncludeLaunchDescription
23+
from launch.launch_description_sources import PythonLaunchDescriptionSource
24+
from launch.substitutions import LaunchConfiguration
25+
from launch_ros.actions import Node
26+
27+
28+
def generate_launch_description():
29+
launch_file_dir = os.path.join(get_package_share_directory('wpr_simulation2'), 'launch')
30+
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
31+
32+
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
33+
34+
world = os.path.join(
35+
get_package_share_directory('wpr_simulation2'),
36+
'worlds',
37+
'robocup_home.world'
38+
)
39+
40+
behavior_tree = os.path.join(
41+
get_package_share_directory('wpr_simulation2'),
42+
'config',
43+
'behavior_tree.xml'
44+
)
45+
46+
map_dir = os.path.join(
47+
get_package_share_directory('wpr_simulation2'),
48+
'map',
49+
'map.yaml'
50+
)
51+
52+
nav_param_dir = os.path.join(
53+
get_package_share_directory('wpr_simulation2'),
54+
'config',
55+
'nav2_params.yaml'
56+
)
57+
58+
nav2_launch_file_dir = os.path.join(
59+
get_package_share_directory('nav2_bringup'),
60+
'launch'
61+
)
62+
63+
gzserver_cmd = IncludeLaunchDescription(
64+
PythonLaunchDescriptionSource(
65+
os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
66+
),
67+
launch_arguments={'world': world}.items()
68+
)
69+
70+
gzclient_cmd = IncludeLaunchDescription(
71+
PythonLaunchDescriptionSource(
72+
os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
73+
)
74+
)
75+
76+
spawn_robot_cmd = IncludeLaunchDescription(
77+
PythonLaunchDescriptionSource(
78+
os.path.join(launch_file_dir, 'spawn_wpb.launch.py')
79+
)
80+
)
81+
82+
navigation_cmd = IncludeLaunchDescription(
83+
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
84+
launch_arguments={
85+
'map': map_dir,
86+
'use_sim_time': use_sim_time,
87+
'params_file': nav_param_dir}.items(),
88+
)
89+
90+
# Define the parameters for the navigation stack
91+
92+
93+
rviz_cmd = Node(
94+
package='rviz2',
95+
namespace='',
96+
executable='rviz2',
97+
name='rviz2',
98+
arguments=['-d', [os.path.join(get_package_share_directory('wp_map_tools'), 'rviz', 'navi.rviz')]]
99+
)
100+
101+
ld = LaunchDescription()
102+
103+
# Add the commands to the launch description
104+
ld.add_action(gzserver_cmd)
105+
ld.add_action(gzclient_cmd)
106+
ld.add_action(spawn_robot_cmd)
107+
ld.add_action(navigation_cmd)
108+
ld.add_action(rviz_cmd)
109+
110+
return ld

‎launch/navi_waypoint_sim.launch.py

+58
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#!/usr/bin/env python3
2+
#
3+
# Copyright 2023 6-robot.
4+
#
5+
# Licensed under the Apache License, Version 2.0 (the "License");
6+
# you may not use this file except in compliance with the License.
7+
# You may obtain a copy of the License at
8+
#
9+
# http://www.apache.org/licenses/LICENSE-2.0
10+
#
11+
# Unless required by applicable law or agreed to in writing, software
12+
# distributed under the License is distributed on an "AS IS" BASIS,
13+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
# See the License for the specific language governing permissions and
15+
# limitations under the License.
16+
#
17+
# Authors: Zhang Wanjie
18+
19+
import os
20+
from ament_index_python.packages import get_package_share_directory
21+
from launch import LaunchDescription
22+
from launch.actions import IncludeLaunchDescription
23+
from launch.launch_description_sources import PythonLaunchDescriptionSource
24+
from launch.substitutions import LaunchConfiguration
25+
from launch_ros.actions import Node
26+
27+
28+
def generate_launch_description():
29+
launch_file_dir = os.path.join(get_package_share_directory('wp_map_tools'), 'launch')
30+
31+
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
32+
33+
navigation_cmd = IncludeLaunchDescription(
34+
PythonLaunchDescriptionSource(
35+
os.path.join(launch_file_dir, 'marker_navigation.launch.py')
36+
)
37+
)
38+
39+
wp_edit_cmd = Node(
40+
package='wp_map_tools',
41+
executable='wp_edit_node',
42+
name='wp_edit_node'
43+
)
44+
45+
wp_navi_cmd = Node(
46+
package='wp_map_tools',
47+
executable='wp_navi_server',
48+
name='wp_navi_server'
49+
)
50+
51+
ld = LaunchDescription()
52+
53+
# Add the commands to the launch description
54+
ld.add_action(navigation_cmd)
55+
ld.add_action(wp_edit_cmd)
56+
ld.add_action(wp_navi_cmd)
57+
58+
return ld

0 commit comments

Comments
 (0)