first commit

Author: zhangwanjie

CMakeLists.txt

@@ -0,0 +1,172 @@
+cmake_minimum_required(VERSION 3.8)
+project(wp_map_tools)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(interactive_markers REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+find_package(rviz_common REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
+find_package(rviz_ogre_vendor REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(pluginlib REQUIRED)
+
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+include_directories(include)
+include_directories(${QT_INCLUDE_DIRS})
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+ament_export_dependencies(
+  "interactive_markers"
+  "rclcpp"
+  "rclcpp_components"
+  "tf2"
+  "tf2_geometry_msgs"
+  "tf2_ros"
+  "visualization_msgs"
+  "geometry_msgs"
+  "yaml-cpp")
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Waypoint.msg" 
+  "srv/SaveWaypoints.srv"
+  DEPENDENCIES 
+  "geometry_msgs"
+)
+
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# 库
+set(wp_map_plugins_headers_to_moc
+  include/wp_map_tools/add_waypoint_tool.hpp
+)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+
+find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+add_definitions(-DQT_NO_KEYWORDS)
+
+set(library_name wp_map_plugins)
+
+add_library(${library_name} SHARED
+  include/wp_map_tools/add_waypoint_tool.hpp
+  src/add_waypoint_tool.cpp
+  ${wp_map_plugins_headers_to_moc}
+)
+
+set(dependencies
+  geometry_msgs
+  pluginlib
+  Qt5
+  rclcpp
+  rviz_common
+  rviz_default_plugins
+  rviz_ogre_vendor
+  rviz_rendering
+  tf2_geometry_msgs
+  visualization_msgs
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+target_include_directories(${library_name} PUBLIC
+  ${Qt5Widgets_INCLUDE_DIRS}
+  ${OGRE_INCLUDE_DIRS}
+)
+
+target_link_libraries(${library_name}
+  rviz_common::rviz_common
+  ${QT_LIBRARIES}
+  "${cpp_typesupport_target}"
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+# TODO: Make this specific to this project (not rviz default plugins)
+target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
+ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
+
+# prevent pluginlib from using boost
+target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
+
+install(
+  FILES 
+  plugins_description.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(
+  TARGETS ${library_name}
+  EXPORT ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+# 编译节点
+add_executable(wp_edit_node src/wp_edit_node.cpp)
+ament_target_dependencies(wp_edit_node 
+  "rclcpp" 
+  "visualization_msgs"
+  "interactive_markers"
+  "yaml-cpp"
+)
+target_link_libraries(wp_edit_node "yaml-cpp" "${cpp_typesupport_target}")
+
+add_executable(wp_saver src/wp_saver.cpp)
+ament_target_dependencies(wp_saver rclcpp )
+target_link_libraries(wp_saver "${cpp_typesupport_target}")
+
+# 安装节点
+install(DIRECTORY launch meshes rviz msg srv icons
+  DESTINATION share/${PROJECT_NAME})
+
+install(
+  TARGETS
+    wp_edit_node 
+    wp_saver
+  DESTINATION
+    lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

README.md

@@ -0,0 +1,46 @@
+# MapTools工具
+
+## 使用步骤
+
+1. 安装ROS(Humble)
+2. 获取源码:
+```
+cd ~/catkin_ws/src/
+git clone https://github.com/6-robot/wp_map_tools.git
+```
+3. 安装依赖项(根据ROS版本选择其中一个):
+```
+cd ~/ros2_ws/src/wp_map_tools/scripts/
+./install_for_humble.sh
+```
+4. 编译
+```
+cd ~/ros2_ws
+colcon build --symlink-install
+```
+
+## 平台介绍
+MapTools工具是[北京六部工坊科技有限公司](http://www.6-robot.com)为旗下WP系列机器人快速设置地图航点所设计的辅助工具,具有操作简单,效果直观的优点。目前支持启智ROS,启智MANI,启程3,启程4和启明1等型号的机器人.
+![Nav pic](./media/wpb_home_nav.png)
+
+## 操作方法
+
+### 1. 打开地图
+仿真:
+```
+ros2 launch wp_map_tools add_waypoint_sim.launch.py
+```
+![1 pic](./media/map.png)
+
+### 2. 设置航点
+在Rviz工具栏点击"Add Waypoint"按钮可在地图上设置航点。
+![2 pic](./media/toolbar.png)
+![3 pic](./media/add_waypoint.png)
+![4 pic](./media/waypoint.png)
+![MapTools pic](./media/map_tools.png)
+
+### 3. 保存航点
+航点设置完毕后,使用如下指令保存航点:
+```
+ros2 run wp_map_tools wp_saver 
+```

icons/classes/AddCharger.png

@@ -0,0 +1,62 @@
+#ifndef ADD_WAYPOINT_TOOL_H
+#define ADD_WAYPOINT_TOOL_H
+
+#ifndef Q_MOC_RUN  // See: https://bugreports.qt-project.org/browse/QTBUG-22829
+    # include <QObject>
+    # include <rclcpp/rclcpp.hpp>
+    # include <rviz_common/tool.hpp>
+    #include "rviz_default_plugins/tools/pose/pose_tool.hpp"
+    #include <rviz_default_plugins/tools/pose/pose_tool.hpp>
+    #include "rviz_default_plugins/visibility_control.hpp"
+    #include <rviz_common/display_context.hpp>
+    #include <rviz_common/properties/string_property.hpp>
+#endif
+
+#include <wp_map_tools/msg/waypoint.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+
+
+namespace rviz_common
+{
+
+    class DisplayContext;
+
+    namespace properties
+    {
+        class StringProperty;
+    }  // namespace properties
+}  // namespace rviz_common
+
+namespace wp_map_tools
+{
+    class Arrow;
+    class DisplayContext;
+    class StringProperty;
+
+    class RVIZ_DEFAULT_PLUGINS_PUBLIC AddWaypointTool: public rviz_default_plugins::tools::PoseTool
+    {
+        Q_OBJECT
+        public:
+        AddWaypointTool();
+        ~AddWaypointTool() override;
+
+        void onInitialize() override;
+
+        protected:
+        void onPoseSet(double x, double y, double theta) override;
+
+        private Q_SLOTS:
+        void updateTopic();
+
+        private:
+        rclcpp::Node::SharedPtr raw_node;
+        rclcpp::Publisher<wp_map_tools::msg::Waypoint>::SharedPtr pub_;
+        // ros::ServiceClient cliGetWPName;
+        rviz_common::properties::StringProperty* topic_property_;
+    };
+
+}
+
+#endif // ADD_WAYPOINT_TOOL_H

icons/classes/AddWaypointTool.png

@@ -0,0 +1,109 @@
+#!/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.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import launch_ros.actions
+import launch.actions
+import launch.events
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('wp_map_tools'), 'launch')
+
+    # 声明launch文件参数，用于指定map.yaml文件路径
+    map_file = os.path.join(
+        get_package_share_directory('wpr_simulation2'),
+        'map',
+        'map.yaml'
+    )
+
+    # 创建map_server节点
+    map_server_cmd = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': map_file},
+                    {'topic': 'map'},
+                    {'frame_id': 'map'},
+                    {'output': 'screen'},
+                    {'use_sim_time': True}]
+    )
+
+    lifecycle_nodes = ['map_server']
+    use_sim_time = True
+    autostart = True
+
+    start_lifecycle_manager_cmd = launch_ros.actions.Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager',
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[{'use_sim_time': use_sim_time},
+                        {'autostart': autostart},
+                        {'node_names': lifecycle_nodes}])
+
+    map_tf_cmd = launch_ros.actions.Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        output='screen',
+        arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link'])
+
+    wp_edit_cmd = Node(
+            package='wp_map_tools',
+            executable='wp_edit_node',
+            name='wp_edit_node',
+            output='screen'
+        )
+    
+    rviz_cmd = Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', [os.path.join(get_package_share_directory('wp_map_tools'), 'rviz', 'edit.rviz')]]
+        )
+
+    # 创建一个事件处理程序，用于在Rviz2退出时发送一个事件
+    shutdown_handler = launch.actions.RegisterEventHandler(
+        launch.event_handlers.OnProcessExit(
+            target_action=rviz_cmd,
+            on_exit=[
+                launch.actions.LogInfo(
+                    msg=(launch.substitutions.EnvironmentVariable(name='USER'),' closed the RViz2 window')
+                    ),
+                launch.actions.EmitEvent(event=launch.events.Shutdown(reason='Window closed'))
+            ]
+        )
+    )
+
+    ld = LaunchDescription()
+
+    # Add the commands to the launch description
+    ld.add_action(map_server_cmd)
+    ld.add_action(start_lifecycle_manager_cmd)
+    ld.add_action(map_tf_cmd)
+    ld.add_action(wp_edit_cmd)
+    ld.add_action(rviz_cmd)
+    # 添加事件处理程序到LaunchDescription
+    ld.add_action(shutdown_handler)
+
+    return ld