From 983b0f5a0045a02e603d3d8551f787d7f7bed0f3 Mon Sep 17 00:00:00 2001 From: isamu-takagi <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 28 Feb 2019 21:46:43 +0900 Subject: [PATCH] [Feature] Initial Release of Waypoint Editor (#1) --- .gitignore | 4 + README.md | 4 +- ros/.gitignore | 6 + ros/src/waypoint-editor/CMakeLists.txt | 61 +++++++ ros/src/waypoint-editor/README.md | 16 ++ ros/src/waypoint-editor/config/test.rviz | 152 ++++++++++++++++++ ros/src/waypoint-editor/launch/test.launch | 3 + ros/src/waypoint-editor/package.xml | 33 ++++ .../waypoint-editor/plugin_description.xml | 11 ++ .../src/event_capture_client.cpp | 37 +++++ .../src/event_capture_client.hpp | 38 +++++ .../src/event_capture_server.cpp | 61 +++++++ .../src/event_capture_server.hpp | 27 ++++ .../src/event_capture_tool.cpp | 47 ++++++ .../src/event_capture_tool.hpp | 38 +++++ .../waypoint-editor/src/point_cloud_map.cpp | 90 +++++++++++ .../waypoint-editor/src/point_cloud_map.hpp | 31 ++++ ros/src/waypoint-editor/src/types/capture.hpp | 24 +++ .../waypoint-editor/src/types/geometry.hpp | 30 ++++ .../waypoint-editor/src/types/waypoint.hpp | 20 +++ .../src/waypoint_editor_library.cpp | 124 ++++++++++++++ .../src/waypoint_editor_library.hpp | 37 +++++ .../src/waypoint_editor_marker.cpp | 125 ++++++++++++++ .../src/waypoint_editor_marker.hpp | 26 +++ .../src/waypoint_editor_panel.cpp | 93 +++++++++++ .../src/waypoint_editor_panel.hpp | 39 +++++ 26 files changed, 1176 insertions(+), 1 deletion(-) create mode 100644 ros/.gitignore create mode 100644 ros/src/waypoint-editor/CMakeLists.txt create mode 100644 ros/src/waypoint-editor/README.md create mode 100644 ros/src/waypoint-editor/config/test.rviz create mode 100644 ros/src/waypoint-editor/launch/test.launch create mode 100644 ros/src/waypoint-editor/package.xml create mode 100644 ros/src/waypoint-editor/plugin_description.xml create mode 100644 ros/src/waypoint-editor/src/event_capture_client.cpp create mode 100644 ros/src/waypoint-editor/src/event_capture_client.hpp create mode 100644 ros/src/waypoint-editor/src/event_capture_server.cpp create mode 100644 ros/src/waypoint-editor/src/event_capture_server.hpp create mode 100644 ros/src/waypoint-editor/src/event_capture_tool.cpp create mode 100644 ros/src/waypoint-editor/src/event_capture_tool.hpp create mode 100644 ros/src/waypoint-editor/src/point_cloud_map.cpp create mode 100644 ros/src/waypoint-editor/src/point_cloud_map.hpp create mode 100644 ros/src/waypoint-editor/src/types/capture.hpp create mode 100644 ros/src/waypoint-editor/src/types/geometry.hpp create mode 100644 ros/src/waypoint-editor/src/types/waypoint.hpp create mode 100644 ros/src/waypoint-editor/src/waypoint_editor_library.cpp create mode 100644 ros/src/waypoint-editor/src/waypoint_editor_library.hpp create mode 100644 ros/src/waypoint-editor/src/waypoint_editor_marker.cpp create mode 100644 ros/src/waypoint-editor/src/waypoint_editor_marker.hpp create mode 100644 ros/src/waypoint-editor/src/waypoint_editor_panel.cpp create mode 100644 ros/src/waypoint-editor/src/waypoint_editor_panel.hpp diff --git a/.gitignore b/.gitignore index 259148f..519613f 100644 --- a/.gitignore +++ b/.gitignore @@ -30,3 +30,7 @@ *.exe *.out *.app + +# Visual Studio Code +.vscode + diff --git a/README.md b/README.md index 86f1cc2..2c4f199 100644 --- a/README.md +++ b/README.md @@ -1 +1,3 @@ -# rviz-plugins \ No newline at end of file +# Rviz Plugins + +- [Waypoint Editor](ros/src/waypoint-editor/README.md) diff --git a/ros/.gitignore b/ros/.gitignore new file mode 100644 index 0000000..90569e3 --- /dev/null +++ b/ros/.gitignore @@ -0,0 +1,6 @@ +# Catkin Workspace +/build +/devel +/.catkin_workspace +/src/CMakeLists.txt + diff --git a/ros/src/waypoint-editor/CMakeLists.txt b/ros/src/waypoint-editor/CMakeLists.txt new file mode 100644 index 0000000..7ff3307 --- /dev/null +++ b/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} +) diff --git a/ros/src/waypoint-editor/README.md b/ros/src/waypoint-editor/README.md new file mode 100644 index 0000000..574f43d --- /dev/null +++ b/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 | diff --git a/ros/src/waypoint-editor/config/test.rviz b/ros/src/waypoint-editor/config/test.rviz new file mode 100644 index 0000000..f8ed997 --- /dev/null +++ b/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: + 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: + 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 diff --git a/ros/src/waypoint-editor/launch/test.launch b/ros/src/waypoint-editor/launch/test.launch new file mode 100644 index 0000000..d087e32 --- /dev/null +++ b/ros/src/waypoint-editor/launch/test.launch @@ -0,0 +1,3 @@ + + + diff --git a/ros/src/waypoint-editor/package.xml b/ros/src/waypoint-editor/package.xml new file mode 100644 index 0000000..900d11e --- /dev/null +++ b/ros/src/waypoint-editor/package.xml @@ -0,0 +1,33 @@ + + + + waypoint-editor + 0.0.0 + The waypoint-editor package + Apache 2.0 + + Isamu Takagi + + catkin + roscpp + rviz + tf2 + tf2_geometry_msgs + tf2_ros + pcl_ros + sensor_msgs + geometry_msgs + roscpp + rviz + tf2 + tf2_geometry_msgs + tf2_ros + pcl_ros + sensor_msgs + geometry_msgs + + + + + + diff --git a/ros/src/waypoint-editor/plugin_description.xml b/ros/src/waypoint-editor/plugin_description.xml new file mode 100644 index 0000000..2855355 --- /dev/null +++ b/ros/src/waypoint-editor/plugin_description.xml @@ -0,0 +1,11 @@ + + + + The tool for event capturing + + + + The panel for waypoint editing + + + diff --git a/ros/src/waypoint-editor/src/event_capture_client.cpp b/ros/src/waypoint-editor/src/event_capture_client.cpp new file mode 100644 index 0000000..55f23c2 --- /dev/null +++ b/ros/src/waypoint-editor/src/event_capture_client.cpp @@ -0,0 +1,37 @@ +#include "event_capture_client.hpp" + +#include + +namespace rviz_plugins { + +EventCaptureClient::EventCaptureClient(): spinner_(1) +{ + //pub_ = nh_.advertise("/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); + } +} + +} diff --git a/ros/src/waypoint-editor/src/event_capture_client.hpp b/ros/src/waypoint-editor/src/event_capture_client.hpp new file mode 100644 index 0000000..b26de76 --- /dev/null +++ b/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 +#include + +namespace rviz_plugins { + +class EventCaptureClient +{ + public: + + EventCaptureClient(); + ~EventCaptureClient(); + + template + 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 callback_mouse_; +}; + +} + +#endif diff --git a/ros/src/waypoint-editor/src/event_capture_server.cpp b/ros/src/waypoint-editor/src/event_capture_server.cpp new file mode 100644 index 0000000..8ac26a7 --- /dev/null +++ b/ros/src/waypoint-editor/src/event_capture_server.cpp @@ -0,0 +1,61 @@ +#include "event_capture_server.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins +{ + +EventCaptureServer::EventCaptureServer() +{ + pub_ = nh_.advertise("/event_capture/mouse", 10); + //sub_ = nh_.subscribe("/event_capture/command", 10, callback, this); +} + +EventCaptureServer::~EventCaptureServer() +{ + +} + +void EventCaptureServer::send(rviz::ViewportMouseEvent &event) +{ + const auto viewport_x = event.x / static_cast(event.viewport->getActualWidth()); + const auto viewport_y = event.y / static_cast(event.viewport->getActualHeight()); + const auto camera = event.viewport->getCamera(); + const auto campos = camera->getPosition(); + const auto camvec = camera->getDirection(); + const auto ray = camera->getCameraToViewportRay(viewport_x, viewport_y); + const auto raypos = ray.getOrigin(); + const auto rayvec = ray.getDirection(); + + std::vector buttons; + buttons.push_back(event.left()); + buttons.push_back(event.leftDown()); + buttons.push_back(event.leftUp()); + buttons.push_back(event.middle()); + buttons.push_back(event.middleDown()); + buttons.push_back(event.middleUp()); + buttons.push_back(event.right()); + buttons.push_back(event.rightDown()); + buttons.push_back(event.rightUp()); + buttons.push_back(event.alt()); + buttons.push_back(event.control()); + buttons.push_back(event.shift()); + + std_msgs::String msg; + for(const auto& vec : {raypos, rayvec, campos, camvec}) + { + msg.data += std::to_string(vec.x) + ' '; + msg.data += std::to_string(vec.y) + ' '; + msg.data += std::to_string(vec.z) + '\n'; + } + for(const auto& button : buttons) + { + msg.data += std::to_string(button) + ' '; + } + pub_.publish(msg); +} + +} diff --git a/ros/src/waypoint-editor/src/event_capture_server.hpp b/ros/src/waypoint-editor/src/event_capture_server.hpp new file mode 100644 index 0000000..df70e8e --- /dev/null +++ b/ros/src/waypoint-editor/src/event_capture_server.hpp @@ -0,0 +1,27 @@ +#ifndef EVENT_CAPTURE_SERVER_HPP +#define EVENT_CAPTURE_SERVER_HPP + +#include +#include + +namespace rviz_plugins { + +class EventCaptureServer +{ + public: + + EventCaptureServer(); + ~EventCaptureServer(); + + void send(rviz::ViewportMouseEvent& event); + + private: + + ros::NodeHandle nh_; + ros::Publisher pub_; + ros::Subscriber sub_; +}; + +} + +#endif diff --git a/ros/src/waypoint-editor/src/event_capture_tool.cpp b/ros/src/waypoint-editor/src/event_capture_tool.cpp new file mode 100644 index 0000000..01cc83b --- /dev/null +++ b/ros/src/waypoint-editor/src/event_capture_tool.cpp @@ -0,0 +1,47 @@ +#include "event_capture_tool.hpp" + +namespace rviz_plugins { + +EventCapture::EventCapture() +{ + shortcut_key_ = 'c'; + property_.reset(new rviz::FloatProperty("Ground Height")); + getPropertyContainer()->addChild(property_.get()); +} + +EventCapture::~EventCapture() +{ +} + +void EventCapture::onInitialize() +{ + move_tool_.initialize(context_); +} + +void EventCapture::activate() +{ + printf("activate\n"); +} + +void EventCapture::deactivate() +{ + printf("deactivate\n"); +} + +int EventCapture::processMouseEvent(rviz::ViewportMouseEvent& event) +{ + if(event.right() || event.rightDown() || event.rightUp()) + { + event_server_.send(event); + } + else + { + move_tool_.processMouseEvent(event); + } + return Render; +} + +} + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::EventCapture, rviz::Tool) diff --git a/ros/src/waypoint-editor/src/event_capture_tool.hpp b/ros/src/waypoint-editor/src/event_capture_tool.hpp new file mode 100644 index 0000000..d80a4be --- /dev/null +++ b/ros/src/waypoint-editor/src/event_capture_tool.hpp @@ -0,0 +1,38 @@ +#ifndef EVENT_CAPTURE_TOOL_HPP +#define EVENT_CAPTURE_TOOL_HPP + +#include "event_capture_server.hpp" + +#include +#include +#include +#include +#include +#include + +namespace rviz_plugins { + +class EventCapture: public rviz::Tool +{ + Q_OBJECT + + public: + + EventCapture(); + ~EventCapture(); + + void onInitialize() override; + void activate() override; + void deactivate() override; + int processMouseEvent(rviz::ViewportMouseEvent& event) override; + + private: + + EventCaptureServer event_server_; + rviz::MoveTool move_tool_; + std::unique_ptr property_; +}; + +} + +#endif diff --git a/ros/src/waypoint-editor/src/point_cloud_map.cpp b/ros/src/waypoint-editor/src/point_cloud_map.cpp new file mode 100644 index 0000000..5f014c9 --- /dev/null +++ b/ros/src/waypoint-editor/src/point_cloud_map.cpp @@ -0,0 +1,90 @@ +#include "point_cloud_map.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace rviz_plugins { + +PointCloudMap::PointCloudMap() +{ + transform_.transform.translation.x = 0.0; + transform_.transform.translation.y = 0.0; + transform_.transform.translation.z = 0.0; + transform_.transform.rotation.x = 0.0; + transform_.transform.rotation.y = 0.0; + transform_.transform.rotation.z = 0.0; + transform_.transform.rotation.w = 1.0; + ground_height_global_ = 0.0; +} + +bool PointCloudMap::updateMap() +{ + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener(tf_buffer); + try + { + transform_ = tf_buffer.lookupTransform("map", "world", ros::Time(0), ros::Duration(1.0)); + } + catch (tf2::TransformException& exception) + { + ROS_WARN("failed to lookup transform: %s", exception.what()); + } + + const auto point_msg = ros::topic::waitForMessage("/points_map", ros::Duration(1.0)); + if(point_msg) + { + pcl::PointCloud point_map; + pcl::fromROSMsg(*point_msg, point_map); + + ground_height_.clear(); + for(const auto& point : point_map) + { + auto key = std::make_pair(floor(point.x), floor(point.y)); + if(ground_height_.count(key) == 0) + { + ground_height_[key] = point.z; + } + else + { + ground_height_[key] = std::min(ground_height_[key], point.z); + } + } + } + else + { + ROS_WARN("failed to subscribe message: /points_map"); + } +} + +Point PointCloudMap::getGroundPoint(const Ray& ray) +{ + Point gndpos = doTransform(ray.origin); + + auto key = std::make_pair(floor(gndpos.x), floor(gndpos.y)); + if(ground_height_.count(key)) + { + gndpos.z = ground_height_[key]; + } + else + { + gndpos.z = ground_height_global_; + } + return gndpos; +} + +Point PointCloudMap::doTransform(const Point& point) +{ + geometry_msgs::Point in, out; + in.x = point.x; + in.y = point.y; + in.z = point.z; + tf2::doTransform(in, out, transform_); + return Point{out.x, out.y, out.z}; +} + +} diff --git a/ros/src/waypoint-editor/src/point_cloud_map.hpp b/ros/src/waypoint-editor/src/point_cloud_map.hpp new file mode 100644 index 0000000..c3cbeca --- /dev/null +++ b/ros/src/waypoint-editor/src/point_cloud_map.hpp @@ -0,0 +1,31 @@ +#ifndef POINT_CLOUD_MAP_HPP +#define POINT_CLOUD_MAP_HPP + +#include "types/geometry.hpp" +#include +#include + +namespace rviz_plugins { + +class PointCloudMap +{ + public: + + PointCloudMap(); + ~PointCloudMap() = default; + + bool updateMap(); + Point getGroundPoint(const Ray& ray); + + private: + + Point doTransform(const Point& point); + + geometry_msgs::TransformStamped transform_; + float ground_height_global_; + std::map,float> ground_height_; +}; + +} + +#endif diff --git a/ros/src/waypoint-editor/src/types/capture.hpp b/ros/src/waypoint-editor/src/types/capture.hpp new file mode 100644 index 0000000..d23b6d5 --- /dev/null +++ b/ros/src/waypoint-editor/src/types/capture.hpp @@ -0,0 +1,24 @@ +#ifndef TYPES_CAPTURE_HPP +#define TYPES_CAPTURE_HPP + +#include "types/geometry.hpp" + +struct MouseEvent +{ + Ray select; + Ray camera; + bool left; + bool left_down; + bool left_up; + bool middle; + bool middle_down; + bool middle_up; + bool right; + bool right_down; + bool right_up; + bool ctrl; + bool shift; + bool alt; +}; + +#endif diff --git a/ros/src/waypoint-editor/src/types/geometry.hpp b/ros/src/waypoint-editor/src/types/geometry.hpp new file mode 100644 index 0000000..d502fae --- /dev/null +++ b/ros/src/waypoint-editor/src/types/geometry.hpp @@ -0,0 +1,30 @@ +#ifndef TYPES_GEOMETRY_HPP +#define TYPES_GEOMETRY_HPP + +struct Point +{ + double x; + double y; + double z; +}; + +struct Ray +{ + Point origin; + Point direction; +}; + +#include + +inline Point operator-(const Point& a, const Point& b) +{ + return Point{a.x - b.x, a.y - b.y, a.z - b.z}; +} + +inline double geometry_distance(const Point& a, const Point& b) +{ + Point d = a - b; + return sqrt((d.x * d.x) + (d.y * d.y) + (d.z * d.z)); +} + +#endif diff --git a/ros/src/waypoint-editor/src/types/waypoint.hpp b/ros/src/waypoint-editor/src/types/waypoint.hpp new file mode 100644 index 0000000..8e54eed --- /dev/null +++ b/ros/src/waypoint-editor/src/types/waypoint.hpp @@ -0,0 +1,20 @@ +#ifndef TYPES_WAYPOINT_HPP +#define TYPES_WAYPOINT_HPP + +#include +#include "types/geometry.hpp" + +struct Waypoint +{ + Point pos; + double yaw; + double vel; + int change; + int event; + int steer; + int stop; +}; + +using Waypoints = std::vector; + +#endif diff --git a/ros/src/waypoint-editor/src/waypoint_editor_library.cpp b/ros/src/waypoint-editor/src/waypoint_editor_library.cpp new file mode 100644 index 0000000..9d69598 --- /dev/null +++ b/ros/src/waypoint-editor/src/waypoint_editor_library.cpp @@ -0,0 +1,124 @@ +#include "waypoint_editor_library.hpp" + +#include +#include +#include +#include + +namespace rviz_plugins { + +WaypointEditorLibrary::WaypointEditorLibrary() +{ + select_radius_ = 0.5; +} + +const Waypoints& WaypointEditorLibrary::get() const +{ + return waypoints_; +} + +void WaypointEditorLibrary::add(const Point& point) +{ + if(waypoints_.empty()) + { + Waypoint waypoint; + waypoint.pos = point; + waypoints_.push_back(waypoint); + } + else + { + Point origin, vector; + origin = waypoints_.back().pos; + vector.x = point.x - origin.x; + vector.y = point.y - origin.y; + vector.z = point.z - origin.z; + + int loop = static_cast(std::hypot(vector.x, vector.y)) + 1; + for(int i = 1; i <= loop; ++i) + { + Waypoint waypoint; + waypoint.pos.x = origin.x + (vector.x * i / loop); + waypoint.pos.y = origin.y + (vector.y * i / loop); + waypoint.pos.z = origin.z + (vector.z * i / loop); + waypoints_.push_back(waypoint); + } + } +} + +void WaypointEditorLibrary::select(const Point& point) +{ + double min_distance = 1e+10; + selected_ = nullptr; + + for(auto& waypoint : waypoints_) + { + double distance = geometry_distance(point, waypoint.pos); + if((distance < select_radius_) && (distance < min_distance)) + { + min_distance = distance; + selected_ = &waypoint; + } + } +} + +void WaypointEditorLibrary::move(const Point& point) +{ + if(selected_) + { + selected_->pos = point; + } +} + +void WaypointEditorLibrary::release() +{ + selected_ = nullptr; +} + +void WaypointEditorLibrary::load(const std::string& filepath) +{ + std::ifstream ifs(filepath.c_str()); + if(!ifs) { return; } + + std::string line, cell; + getline(ifs, line); // Skip header line + + waypoints_.clear(); + while(getline(ifs, line), ifs) + { + std::istringstream iss(line); + Waypoint w; + getline(iss, cell, ','); w.pos.x = std::stod(cell); + getline(iss, cell, ','); w.pos.y = std::stod(cell); + getline(iss, cell, ','); w.pos.z = std::stod(cell); + getline(iss, cell, ','); w.yaw = std::stod(cell); + getline(iss, cell, ','); w.vel = std::stod(cell); + getline(iss, cell, ','); w.change = std::stoi(cell); + getline(iss, cell, ','); w.event = std::stoi(cell); + getline(iss, cell, ','); w.steer = std::stoi(cell); + getline(iss, cell, ','); w.stop = std::stoi(cell); + waypoints_.push_back(w); + } +} + +void WaypointEditorLibrary::save(const std::string& filepath) +{ + std::ofstream ofs(filepath.c_str()); + if(!ofs) { return; } + + ofs << "x,y,z,yaw,velocity,change_flag,event_flag,steering_flag,stop_flag" << std::endl; + ofs << std::fixed << std::setprecision(4); + for(const auto& waypoint : waypoints_) + { + ofs << waypoint.pos.x << ','; + ofs << waypoint.pos.y << ','; + ofs << waypoint.pos.z << ','; + ofs << waypoint.yaw << ','; + ofs << waypoint.vel << ','; + ofs << waypoint.change << ','; + ofs << waypoint.event << ','; + ofs << waypoint.steer << ','; + ofs << waypoint.stop << std::endl; + } +} + +} diff --git a/ros/src/waypoint-editor/src/waypoint_editor_library.hpp b/ros/src/waypoint-editor/src/waypoint_editor_library.hpp new file mode 100644 index 0000000..96f8154 --- /dev/null +++ b/ros/src/waypoint-editor/src/waypoint_editor_library.hpp @@ -0,0 +1,37 @@ +#ifndef WAYPOINT_EDITOR_LIBRARY_HPP +#define WAYPOINT_EDITOR_LIBRARY_HPP + +#include "types/geometry.hpp" +#include "types/waypoint.hpp" + +#include + +namespace rviz_plugins { + +class WaypointEditorLibrary +{ + public: + + WaypointEditorLibrary(); + ~WaypointEditorLibrary() = default; + + void add(const Point& point); + void select(const Point& point); + void move(const Point& point); + void release(); + + const Waypoints& get() const; + void load(const std::string& filepath); + void save(const std::string& filepath); + + private: + + Waypoints waypoints_; + Waypoint* selected_; + + double select_radius_; +}; + +} + +#endif diff --git a/ros/src/waypoint-editor/src/waypoint_editor_marker.cpp b/ros/src/waypoint-editor/src/waypoint_editor_marker.cpp new file mode 100644 index 0000000..3cfe469 --- /dev/null +++ b/ros/src/waypoint-editor/src/waypoint_editor_marker.cpp @@ -0,0 +1,125 @@ +#include "waypoint_editor_marker.hpp" +#include + +namespace rviz_plugins { + +WaypointEditorMarker::WaypointEditorMarker() +{ + pub_ = nh_.advertise("/waypoint_editor/markers", 1); +} + +void WaypointEditorMarker::publish(const Waypoints& waypoints) +{ + const std::string frame = "/map"; + int marker_id = -1; + visualization_msgs::MarkerArray msg; + + // Line + { + visualization_msgs::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "link"; + marker.id = ++marker_id; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(); + + marker.scale.x = 0.1; + marker.scale.y = 0.0; + marker.scale.z = 0.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color.r = 0.0f; + marker.color.g = 0.5f; + marker.color.b = 0.0f; + marker.color.a = 1.0f; + + for(const auto& waypoint : waypoints) + { + geometry_msgs::Point p; + p.x = waypoint.pos.x; + p.y = waypoint.pos.y; + p.z = waypoint.pos.z; + marker.points.push_back(p); + } + msg.markers.push_back(marker); + } + + // Point + { + visualization_msgs::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "node"; + marker.id = ++marker_id; + marker.type = visualization_msgs::Marker::POINTS; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(); + + marker.scale.x = 0.3; + marker.scale.y = 0.3; + marker.scale.z = 0.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0f; + + for(const auto& waypoint : waypoints) + { + geometry_msgs::Point p; + p.x = waypoint.pos.x; + p.y = waypoint.pos.y; + p.z = waypoint.pos.z; + marker.points.push_back(p); + } + + msg.markers.push_back(marker); + } + + + // Text + int csv_row = 1; + for(const auto& waypoint : waypoints) + { + visualization_msgs::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = ros::Time::now(); + marker.ns = "text"; + marker.id = ++marker_id; + marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = ros::Duration(); + + marker.text = std::to_string(++csv_row); + + marker.pose.position.x = waypoint.pos.x; + marker.pose.position.y = waypoint.pos.y; + marker.pose.position.z = waypoint.pos.z + 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 0.5; + + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + + msg.markers.push_back(marker); +} + + pub_.publish(msg); +} + +} diff --git a/ros/src/waypoint-editor/src/waypoint_editor_marker.hpp b/ros/src/waypoint-editor/src/waypoint_editor_marker.hpp new file mode 100644 index 0000000..860db41 --- /dev/null +++ b/ros/src/waypoint-editor/src/waypoint_editor_marker.hpp @@ -0,0 +1,26 @@ +#ifndef WAYPOINT_EDITOR_MARKER_HPP +#define WAYPOINT_EDITOR_MARKER_HPP + +#include "types/waypoint.hpp" +#include + +namespace rviz_plugins { + +class WaypointEditorMarker +{ + public: + + WaypointEditorMarker(); + ~WaypointEditorMarker() = default; + + void publish(const Waypoints& waypoints); + + private: + + ros::NodeHandle nh_; + ros::Publisher pub_; +}; + +} + +#endif diff --git a/ros/src/waypoint-editor/src/waypoint_editor_panel.cpp b/ros/src/waypoint-editor/src/waypoint_editor_panel.cpp new file mode 100644 index 0000000..07186eb --- /dev/null +++ b/ros/src/waypoint-editor/src/waypoint_editor_panel.cpp @@ -0,0 +1,93 @@ +#include "waypoint_editor_panel.hpp" + +#include +#include +#include + +namespace rviz_plugins { + +WaypointEditor::WaypointEditor() +{ + auto layout = new QVBoxLayout(); + setLayout(layout); + + auto load_button = new QPushButton("Load"); + layout->addWidget(load_button); + connect(load_button, &QPushButton::clicked, this, &WaypointEditor::load_waypoints); + + auto save_button = new QPushButton("Save"); + layout->addWidget(save_button); + connect(save_button, &QPushButton::clicked, this, &WaypointEditor::save_waypoints); + + layout->addStretch(); +} + +WaypointEditor::~WaypointEditor() +{ + +} + +void WaypointEditor::onInitialize() +{ + point_cloud_map_.updateMap(); + capture_client_.setMouseEvent(this, &WaypointEditor::processMouseEvent); +} + +void WaypointEditor::load_waypoints() +{ + QString filepath = QFileDialog::getOpenFileName(this); + if(!filepath.isEmpty()) + { + waypoint_editor_.load(filepath.toStdString()); + waypoint_viewer_.publish(waypoint_editor_.get()); + } +} + +void WaypointEditor::save_waypoints() +{ + QString filepath = QFileDialog::getSaveFileName(this); + if(!filepath.isEmpty()) + { + waypoint_editor_.save(filepath.toStdString()); + } +} + +void WaypointEditor::processMouseEvent(const MouseEvent& event) +{ + if(event.shift) + { + if(event.right_down) + { + Point gndpos = point_cloud_map_.getGroundPoint(event.select); + waypoint_editor_.add(gndpos); + waypoint_viewer_.publish(waypoint_editor_.get()); + } + } + else + { + if(event.right_down) + { + Point gndpos = point_cloud_map_.getGroundPoint(event.select); + waypoint_editor_.select(gndpos); + } + else if(event.right) + { + Point gndpos = point_cloud_map_.getGroundPoint(event.select); + waypoint_editor_.move(gndpos); + waypoint_viewer_.publish(waypoint_editor_.get()); + } + else if(event.right_up) + { + waypoint_editor_.release(); + } + else + { + + } + } +} + +} + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::WaypointEditor, rviz::Panel) diff --git a/ros/src/waypoint-editor/src/waypoint_editor_panel.hpp b/ros/src/waypoint-editor/src/waypoint_editor_panel.hpp new file mode 100644 index 0000000..8c0286a --- /dev/null +++ b/ros/src/waypoint-editor/src/waypoint_editor_panel.hpp @@ -0,0 +1,39 @@ +#ifndef WAYPOINT_EDITOR_PANEL_HPP +#define WAYPOINT_EDITOR_PANEL_HPP + +#include "event_capture_client.hpp" +#include "point_cloud_map.hpp" +#include "waypoint_editor_library.hpp" +#include "waypoint_editor_marker.hpp" + +#include +#include + +namespace rviz_plugins { + +class WaypointEditor: public rviz::Panel +{ + Q_OBJECT + + public: + + WaypointEditor(); + ~WaypointEditor(); + + void onInitialize() override; + + private: + + void load_waypoints(); + void save_waypoints(); + void processMouseEvent(const MouseEvent& event); + + EventCaptureClient capture_client_; + PointCloudMap point_cloud_map_; + WaypointEditorLibrary waypoint_editor_; + WaypointEditorMarker waypoint_viewer_; +}; + +} + +#endif