[Feature] Initial Release of Waypoint Editor (#1)

.gitignore

@@ -30,3 +30,7 @@
 *.exe
 *.out
 *.app
+
+# Visual Studio Code
+.vscode
+

README.md

@@ -1 +1,3 @@
-# rviz-plugins
+# Rviz Plugins
+
+- [Waypoint Editor](ros/src/waypoint-editor/README.md)

ros/.gitignore

@@ -0,0 +1,6 @@
+# Catkin Workspace
+/build
+/devel
+/.catkin_workspace
+/src/CMakeLists.txt
+

ros/src/waypoint-editor/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(waypoint-editor)
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rviz
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  std_msgs
+  geometry_msgs
+  pcl_ros
+)
+
+find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+add_definitions(-DQT_NO_KEYWORDS)
+
+catkin_package(
+  CATKIN_DEPENDS roscpp rviz
+)
+
+include_directories(
+  src
+  ${catkin_INCLUDE_DIRS}
+  ${Qt5_INCLUDE_DIRS}
+)
+
+qt5_wrap_cpp(QT_HEADER_FILES
+  src/event_capture_server.hpp
+  src/event_capture_client.hpp
+  src/event_capture_tool.hpp
+  src/point_cloud_map.hpp
+  src/waypoint_editor_library.hpp
+  src/waypoint_editor_marker.hpp
+  src/waypoint_editor_panel.hpp
+)
+
+set(QT_SOURCE_FILES
+  src/event_capture_server.cpp
+  src/event_capture_client.cpp
+  src/event_capture_tool.cpp
+  src/point_cloud_map.cpp
+  src/waypoint_editor_library.cpp
+  src/waypoint_editor_marker.cpp
+  src/waypoint_editor_panel.cpp
+)
+
+add_library(${PROJECT_NAME}
+  ${QT_HEADER_FILES}
+  ${QT_SOURCE_FILES}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES} ${QT_LIBRARIES}
+)
+
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)

ros/src/waypoint-editor/README.md

@@ -0,0 +1,16 @@
+# Waypoint Editor
+
+## Setup
+Launch rviz window and setup by following.
+1. Add "EventCapture Tool"
+1. Add "WaypointEditor Panel"
+1. Set view type to "TopDownOrtho"
+1. Activate "EventCapture Tool" (shortcut is c key)
+
+## Usage
+| Action | Key |
+|--------|-----|
+| Move   | right mouse button         |
+| Add    | shift + right mouse button |
+| Load   | click load button on panel |
+| Save   | click save button on panel |

ros/src/waypoint-editor/config/test.rviz

@@ -0,0 +1,152 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 753
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_plugins::WaypointEditor
+    Name: WaypointEditor
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /waypoint_editor/markers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Alpha: 0.100000001
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.00999999978
+      Style: Points
+      Topic: /points_map
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+    - Class: rviz_plugins::EventCapture
+      Ground Height: 0
+  Value: true
+  Views:
+    Current:
+      Angle: 0
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Scale: 61.3039436
+      Target Frame: <Fixed Frame>
+      Value: TopDownOrtho (rviz)
+      X: 0.162051603
+      Y: 0.190646112
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1052
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000017900000386fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e00000386000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001bb00000386fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002e000000c5000000b700fffffffb0000001c0057006100790070006f0069006e00740045006400690074006f007201000000fa000002ba0000004c00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000070c00000040fc0100000002fb0000000800540069006d006501000000000000070c0000038300fffffffb0000000800540069006d00650100000000000004500000000000000000000003ca0000038600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  WaypointEditor:
+    collapsed: false
+  Width: 1804
+  X: 756
+  Y: 353

ros/src/waypoint-editor/launch/test.launch

@@ -0,0 +1,3 @@
+<launch>
+    <node name="rviz" type="rviz" pkg="rviz" args="-d $(find waypoint-editor)/config/test.rviz" output="screen"/>
+</launch>

ros/src/waypoint-editor/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<package format="2">
+
+  <name>waypoint-editor</name>
+  <version>0.0.0</version>
+  <description>The waypoint-editor package</description>
+  <license>Apache 2.0</license>
+
+  <maintainer email="[email protected]">Isamu Takagi</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rviz</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <export>
+    <rviz plugin="${prefix}/plugin_description.xml"/>
+  </export>
+
+</package>

ros/src/waypoint-editor/plugin_description.xml

@@ -0,0 +1,11 @@
+<library path="lib/libwaypoint-editor">
+
+  <class type="rviz_plugins::EventCapture" base_class_type="rviz::Tool">
+    <description>The tool for event capturing</description>
+  </class>
+
+  <class type="rviz_plugins::WaypointEditor" base_class_type="rviz::Panel">
+    <description>The panel for waypoint editing</description>
+  </class>
+
+</library>

ros/src/waypoint-editor/src/event_capture_client.cpp

@@ -0,0 +1,37 @@
+#include "event_capture_client.hpp"
+
+#include <sstream>
+
+namespace rviz_plugins {
+
+EventCaptureClient::EventCaptureClient(): spinner_(1)
+{
+    //pub_ = nh_.advertise<std_msgs::String>("/event_capture/command", 10);
+    sub_ = nh_.subscribe("/event_capture/mouse", 10, &EventCaptureClient::callbackMouseEvent, this);
+    spinner_.start();
+}
+
+EventCaptureClient::~EventCaptureClient()
+{
+    spinner_.stop();
+}
+
+void EventCaptureClient::callbackMouseEvent(const std_msgs::String& msg)
+{
+    if(callback_mouse_)
+    {
+        std::istringstream iss(msg.data);
+        MouseEvent event;
+        iss >> event.select.origin.x    >> event.select.origin.y    >> event.select.origin.z;
+        iss >> event.select.direction.x >> event.select.direction.y >> event.select.direction.z;
+        iss >> event.camera.origin.x    >> event.camera.origin.y    >> event.camera.origin.z;
+        iss >> event.camera.direction.x >> event.camera.direction.y >> event.camera.direction.z;
+        iss >> event.left   >> event.left_down   >> event.left_up;
+        iss >> event.middle >> event.middle_down >> event.middle_up;
+        iss >> event.right  >> event.right_down  >> event.right_up;
+        iss >> event.alt    >> event.ctrl        >> event.shift;
+        callback_mouse_(event);
+    }
+}
+
+}

ros/src/waypoint-editor/src/event_capture_client.hpp

@@ -0,0 +1,38 @@
+#ifndef EVENT_CAPTURE_CLIENT_HPP
+#define EVENT_CAPTURE_CLIENT_HPP
+
+#include "types/capture.hpp"
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+
+namespace rviz_plugins {
+
+class EventCaptureClient
+{
+    public:
+
+        EventCaptureClient();
+        ~EventCaptureClient();
+
+        template<class T>
+        void setMouseEvent(T* obj, void(T::*func)(const MouseEvent& event))
+        {
+            callback_mouse_ = [=](const MouseEvent& event){ (obj->*func)(event); };
+        }
+
+    private:
+
+        void callbackMouseEvent(const std_msgs::String& msg);
+        //void callbackKeyEvent(const std_msgs::String& msg);
+
+        ros::AsyncSpinner spinner_;
+        ros::NodeHandle nh_;
+        ros::Publisher  pub_;
+        ros::Subscriber sub_;
+
+        std::function<void(const MouseEvent& event)> callback_mouse_;
+};
+
+}
+
+#endif