add demo_8

CMakeLists.txt

@@ -75,6 +75,11 @@ ament_target_dependencies(6_imu_behavior
 "rclcpp" "sensor_msgs" "tf2" "geometry_msgs"
 )
 
+add_executable(8_waypoint_navigation demo_cpp/8_waypoint_navigation.cpp)
+ament_target_dependencies(8_waypoint_navigation 
+"rclcpp" "std_msgs"
+)
+
 add_executable(demo_mani_ctrl demo_cpp/demo_mani_ctrl.cpp)
 ament_target_dependencies(demo_mani_ctrl
 "rclcpp" "sensor_msgs"
@@ -112,6 +117,7 @@ install(
     4_velocity_command
     5_lidar_data 5_lidar_behavior
     6_imu_data 6_imu_behavior
+    8_waypoint_navigation
     demo_mani_ctrl  
     demo_nav2_client
     keyboard_vel_cmd

demo_cpp/8_waypoint_navigation.cpp

@@ -0,0 +1,22 @@
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("waypoint_navigation_node");
+
+  auto navigation_pub = node->create_publisher<std_msgs::msg::String>("/waterplus/navi_waypoint", 10);
+
+  rclcpp::sleep_for(std::chrono::milliseconds(1000));
+
+  std_msgs::msg::String waypoint_msg;
+  waypoint_msg.data = "1";
+
+  navigation_pub->publish(waypoint_msg);
+
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+
+  return 0;
+}

launch/map_tools.launch.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python3
+#
+# Copyright 2023 6-robot.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# Authors: Zhang Wanjie
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    
+    launch_file_dir = os.path.join(get_package_share_directory('wpr_simulation2'), 'launch')
+
+    home_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robocup_home.launch.py')
+        )
+    )
+
+    behavior_tree = os.path.join(
+        get_package_share_directory('wpr_simulation2'),
+        'config',
+        'behavior_tree.xml'
+    )
+
+    map_file = os.path.join(
+        get_package_share_directory('wpr_simulation2'),
+        'map',
+        'map.yaml'
+    )
+    
+    nav_param_file = os.path.join(
+        get_package_share_directory('wpr_simulation2'),
+        'config',
+        'nav2_params.yaml'
+    )
+
+    nav2_launch_dir = os.path.join(
+        get_package_share_directory('nav2_bringup'), 
+        'launch'
+    )
+
+    navigation_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([nav2_launch_dir, '/bringup_launch.py']),
+        launch_arguments={
+            'map': map_file,
+            'use_sim_time': use_sim_time,
+            'params_file': nav_param_file}.items(),
+    )
+
+    rviz_cmd = Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', [os.path.join(get_package_share_directory('wp_map_tools'), 'rviz', 'navi.rviz')]]
+        )
+
+    wp_edit_cmd = Node(
+            package='wp_map_tools',
+            executable='wp_edit_node',
+            name='wp_edit_node'
+        )
+    
+    wp_navi_server_cmd = Node(
+            package='wp_map_tools',
+            executable='wp_navi_server',
+            name='wp_navi_server'
+        )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(home_cmd)
+    ld.add_action(navigation_cmd)
+    ld.add_action(rviz_cmd)
+
+    ld.add_action(wp_edit_cmd)
+    ld.add_action(wp_navi_server_cmd)
+
+    return ld

launch/navigation.launch.py

@@ -24,17 +24,15 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
-
 def generate_launch_description():
-    launch_file_dir = os.path.join(get_package_share_directory('wpr_simulation2'), 'launch')
-    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
-
     use_sim_time = LaunchConfiguration('use_sim_time', default='true')
     
-    world = os.path.join(
-        get_package_share_directory('wpr_simulation2'),
-        'worlds',
-        'robocup_home.world'
+    launch_file_dir = os.path.join(get_package_share_directory('wpr_simulation2'), 'launch')
+
+    home_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, 'robocup_home.launch.py')
+        )
     )
 
     behavior_tree = os.path.join(
@@ -43,67 +41,42 @@ def generate_launch_description():
         'behavior_tree.xml'
     )
 
-    map_dir = os.path.join(
+    map_file = os.path.join(
         get_package_share_directory('wpr_simulation2'),
         'map',
         'map.yaml'
     )
     
-    nav_param_dir = os.path.join(
+    nav_param_file = os.path.join(
         get_package_share_directory('wpr_simulation2'),
         'config',
         'nav2_params.yaml'
     )
 
-    nav2_launch_file_dir = os.path.join(
+    nav2_launch_dir = os.path.join(
         get_package_share_directory('nav2_bringup'), 
         'launch'
     )
 
-    gzserver_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
-        ),
-        launch_arguments={'world': world}.items()
-    )
-
-    gzclient_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
-        )
-    )
-
-    spawn_robot_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'spawn_wpb.launch.py')
-        )
-    )
-
     navigation_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
+        PythonLaunchDescriptionSource([nav2_launch_dir, '/bringup_launch.py']),
         launch_arguments={
-            'map': map_dir,
+            'map': map_file,
             'use_sim_time': use_sim_time,
-            'params_file': nav_param_dir}.items(),
+            'params_file': nav_param_file}.items(),
     )
 
-    # Define the parameters for the navigation stack
-    
-
     rviz_cmd = Node(
             package='rviz2',
-            namespace='',
             executable='rviz2',
             name='rviz2',
-            arguments=['-d', [os.path.join(get_package_share_directory('wpr_simulation2'), 'rviz', 'nav2.rviz')]]
+            arguments=['-d', [os.path.join(get_package_share_directory('wpr_simulation2'), 'rviz', 'navi.rviz')]]
         )
 
     ld = LaunchDescription()
 
     # Add the commands to the launch description
-    ld.add_action(gzserver_cmd)
-    ld.add_action(gzclient_cmd)
-    ld.add_action(spawn_robot_cmd)
+    ld.add_action(home_cmd)
     ld.add_action(navigation_cmd)
     ld.add_action(rviz_cmd)