├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── demo_node.rviz ├── images └── demo.png ├── include └── ros_viz_tools │ ├── color.h │ └── ros_viz_tools.h ├── launch └── demo_node.launch ├── package.xml └── src ├── color.cpp ├── demo_node.cpp └── ros_viz_tools.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_viz_tools) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | set(catkin_deps 9 | geometry_msgs 10 | roscpp 11 | std_msgs 12 | visualization_msgs 13 | tf2 14 | ) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | ${catkin_deps} 18 | ) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | LIBRARIES ${PROJECT_NAME} 23 | CATKIN_DEPENDS ${catkin_deps} 24 | ) 25 | 26 | # include 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | # library 33 | add_library(${PROJECT_NAME} 34 | src/ros_viz_tools.cpp 35 | src/color.cpp 36 | ) 37 | 38 | target_link_libraries(${PROJECT_NAME} 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | # demo node 43 | add_executable(demo_node 44 | src/demo_node.cpp 45 | ) 46 | 47 | target_link_libraries(demo_node 48 | ${PROJECT_NAME} 49 | ${catkin_LIBRARIES} 50 | ) 51 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Wei Wang 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros_viz_tools 2 | 3 | This package is a visualization tool for easier Rviz [marker](http://wiki.ros.org/rviz/DisplayTypes/Marker) plotting. 4 | 5 | ## Dependencies 6 | 7 | - Tested on Ubuntu 16.04 LTS & ROS kinetic 8 | - tf2_geometry_msgs 9 | 10 | ## Quick Start 11 | 12 | Build this package under your catkin workspace, run demo node and Rviz (frame: `ros_viz_tools`, topic: `demo_marker`) for a quick look: 13 | 14 | ```bash 15 | roslaunch ros_viz_tools demo_node.launch 16 | ``` 17 | 18 | ![demo](./images/demo.png) 19 | 20 | Currently support: 21 | 22 | - Cube list 23 | - Sphere list 24 | - Line Strip 25 | - Line List 26 | - Cylinder 27 | - Cube 28 | - Sphere 29 | - Arrow 30 | - Text 31 | - Frame (Axes-type Pose) 32 | 33 | ## Usage 34 | 35 | Set catkin package dependencies in your `CMakeLists.txt` and `package.xml`, 36 | 37 | ```cmake 38 | # CMakeLists.txt 39 | find_package(catkin REQUIRED COMPONENTS 40 | ... 41 | ros_viz_tools 42 | ) 43 | ``` 44 | 45 | ```xml 46 | 47 | 48 | 49 | ... 50 | ros_viz_tools 51 | ... 52 | 53 | ``` 54 | 55 | Include the header file in your codes, 56 | 57 | ```c++ 58 | #include "ros_viz_tools/ros_viz_tools.h" 59 | ``` 60 | 61 | ### Markers 62 | 63 | Initialize a `RosVizTools` instance named `markers`, 64 | 65 | ```c++ 66 | ros::NodeHandle n; 67 | std::string topic = "demo_marker"; 68 | ros_viz_tools::RosVizTools markers(n, topic); 69 | ``` 70 | 71 | Create a new marker and append it to `markers`. Let's take cube list marker for example. 72 | 73 | ```c++ 74 | // set marker frame id, namespace and id 75 | std::string frame_id = "ros_viz_tools"; 76 | std::string ns = "cube_list"; 77 | int id = 0; 78 | ``` 79 | 80 | You can initialize a new marker by two approaches: 81 | 82 | ```c++ 83 | // intialize new marker by calling static member function in RosVizTools directly (recommended) 84 | visualization_msgs::Marker marker = ros_viz_tools::RosVizTools::newCubeList(0.5, ns, id, ros_viz_tools::WHITE, frame_id); 85 | // or by accessing the function through the instance 86 | visualization_msgs::Marker marker = markers.newCubeList(0.5, ns, id, ros_viz_tools::WHITE, frame_id); 87 | ``` 88 | 89 | If the new marker involves a list (cube list, sphere list, line list or line strip), you also need to set a point list. 90 | 91 | ```c++ 92 | // modify marker, cube list, for example, also needs a point list. 93 | for (int i = 0; i < 10; ++i) { 94 | geometry_msgs::Point p; 95 | p.x = i; 96 | p.y = pow(p.x, 2.0); 97 | p.z = 1.0; 98 | marker.points.push_back(p); 99 | std_msgs::ColorRGBA color = ros_viz_tools::newColorRGBA(randRGB(e), randRGB(e), randRGB(e)); 100 | marker.colors.push_back(color); 101 | } 102 | ``` 103 | 104 | Append new marker to `RosVizTools` instance `markers`: 105 | 106 | ```c++ 107 | // append to markers 108 | markers.append(marker); 109 | ``` 110 | 111 | At the end, call `publish()` function. 112 | 113 | ```c++ 114 | markers.publish(); 115 | ``` 116 | 117 | Then you can open Rviz and see the markers published in the frame `ros_viz_tools` and topic `demo_marker`. Don't forget clear your markers at the beginning of every loop: 118 | 119 | ```c++ 120 | markers.clear(); 121 | ``` 122 | 123 | You can see [demo_node.cpp](./src/demo_node.cpp) for better understanding of the usage for each marker type. 124 | 125 | ### Colors 126 | 127 | To support colorful marker plotting, `ros_viz_tools` also defines functions and class for easier color settings. Now there are two approaches supported for generating colors: 128 | 129 | * function `newColorRGBA` (also with some pre-defined colors in `color.h`) or 130 | * class `ColorMap` (See `demo_node.cpp` for examples) 131 | 132 | ## License 133 | 134 | This repository licensed under the [MIT License](./LICENSE). -------------------------------------------------------------------------------- /config/demo_node.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /MarkerArray1 9 | - /MarkerArray1/Namespaces1 10 | Splitter Ratio: 0.5 11 | Tree Height: 584 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/MarkerArray 55 | Enabled: true 56 | Marker Topic: /demo_marker 57 | Name: MarkerArray 58 | Namespaces: 59 | arrow: true 60 | axes: true 61 | colormap: true 62 | cube: true 63 | cube_list: true 64 | cylinder: true 65 | line_strip: true 66 | sphere_list: true 67 | text: true 68 | Queue Size: 100 69 | Value: true 70 | Enabled: true 71 | Global Options: 72 | Background Color: 48; 48; 48 73 | Default Light: true 74 | Fixed Frame: ros_viz_tools 75 | Frame Rate: 30 76 | Name: root 77 | Tools: 78 | - Class: rviz/Interact 79 | Hide Inactive Objects: true 80 | - Class: rviz/MoveCamera 81 | - Class: rviz/Select 82 | - Class: rviz/FocusCamera 83 | - Class: rviz/Measure 84 | - Class: rviz/SetInitialPose 85 | Topic: /initialpose 86 | - Class: rviz/SetGoal 87 | Topic: /move_base_simple/goal 88 | - Class: rviz/PublishPoint 89 | Single click: true 90 | Topic: /clicked_point 91 | Value: true 92 | Views: 93 | Current: 94 | Class: rviz/Orbit 95 | Distance: 21.9013844 96 | Enable Stereo Rendering: 97 | Stereo Eye Separation: 0.0599999987 98 | Stereo Focal Distance: 1 99 | Swap Stereo Eyes: false 100 | Value: false 101 | Focal Point: 102 | X: 0.20029375 103 | Y: 4.09143639 104 | Z: 1.67812288 105 | Focal Shape Fixed Size: true 106 | Focal Shape Size: 0.0500000007 107 | Invert Z Axis: false 108 | Name: Current View 109 | Near Clip Distance: 0.00999999978 110 | Pitch: 0.264797717 111 | Target Frame: 112 | Value: Orbit (rviz) 113 | Yaw: 5.07039976 114 | Saved: ~ 115 | Window Geometry: 116 | Displays: 117 | collapsed: false 118 | Height: 865 119 | Hide Left Dock: false 120 | Hide Right Dock: true 121 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002d7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002d7000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f50000003efc0100000002fb0000000800540069006d00650100000000000004f50000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000385000002d700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 122 | Selection: 123 | collapsed: false 124 | Time: 125 | collapsed: false 126 | Tool Properties: 127 | collapsed: false 128 | Views: 129 | collapsed: true 130 | Width: 1269 131 | X: 495 132 | Y: 108 133 | -------------------------------------------------------------------------------- /images/demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Magic-wei/ros_viz_tools/57fcf3e94bc586224f13106f83fd7b98bce86086/images/demo.png -------------------------------------------------------------------------------- /include/ros_viz_tools/color.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2019 Wei Wang (wei.wang.bit@outlook.com) 2 | 3 | #ifndef ROS_VIZ_TOOLS_COLOR_H 4 | #define ROS_VIZ_TOOLS_COLOR_H 5 | 6 | #include 7 | #include 8 | 9 | namespace ros_viz_tools { 10 | 11 | using std_msgs::ColorRGBA; 12 | 13 | ColorRGBA newColorRGBA(uint8_t red, uint8_t green, uint8_t blue, double alpha = 1.0); 14 | ColorRGBA newColorRGBADouble(double red, double green, double blue, double alpha = 1.0); 15 | 16 | // pre-defined color 17 | const ColorRGBA WHITE = newColorRGBA(255, 255, 255); 18 | const ColorRGBA BLACK = newColorRGBA( 0, 0, 0); 19 | const ColorRGBA RED = newColorRGBA(255, 0, 0); 20 | const ColorRGBA GREEN = newColorRGBA( 0, 255, 0); 21 | const ColorRGBA BLUE = newColorRGBA( 0, 0, 255); 22 | const ColorRGBA YELLOW = newColorRGBA(255, 255, 0); 23 | const ColorRGBA CYAN = newColorRGBA( 0, 255, 255); 24 | const ColorRGBA MAGENTA = newColorRGBA(255, 0, 255); 25 | const ColorRGBA GRAY = newColorRGBA(128, 128, 128); 26 | const ColorRGBA PURPLE = newColorRGBA(128, 0, 128); 27 | const ColorRGBA PINK = newColorRGBA(255, 192, 203); 28 | const ColorRGBA LIGHT_BLUE = newColorRGBA(173, 216, 230); 29 | const ColorRGBA LIME_GREEN = newColorRGBA( 50, 205, 50); 30 | const ColorRGBA SLATE_GRAY = newColorRGBA(112, 128, 144); 31 | 32 | class ColorMap { 33 | public: 34 | ColorMap(const std::vector &color_list); 35 | size_t operator()(double value); 36 | static size_t colorRGB2Hex(uint8_t red, uint8_t green, uint8_t blue); 37 | static void colorHex2RGB(const size_t &color_hex, uint8_t &red, uint8_t &green, uint8_t &blue); 38 | static void linspaceColorRGBinHex(size_t start, size_t end, size_t num, std::vector &color_list); 39 | 40 | private: 41 | static size_t colorInterpolation(size_t start, size_t end, double scale); 42 | 43 | std::vector value_list_; 44 | std::vector color_list_; 45 | }; 46 | 47 | // TODO: predefined colormap 48 | 49 | } // namespace ros_viz_tools 50 | 51 | #endif //ROS_VIZ_TOOLS_COLOR_H 52 | -------------------------------------------------------------------------------- /include/ros_viz_tools/ros_viz_tools.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2019 Wei Wang (wei.wang.bit@outlook.com) 2 | 3 | #ifndef ROS_VIZ_TOOLS_H 4 | #define ROS_VIZ_TOOLS_H 5 | 6 | #include "ros_viz_tools/color.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace ros_viz_tools { 20 | 21 | using visualization_msgs::Marker; 22 | using visualization_msgs::MarkerArray; 23 | using std_msgs::ColorRGBA; 24 | 25 | class RosVizTools { 26 | public: 27 | RosVizTools(const ros::NodeHandle &nh, const std::string &topic); 28 | 29 | void publish(); 30 | 31 | void clear(); 32 | 33 | void append(const Marker &marker); 34 | 35 | static Marker newMaker(const geometry_msgs::Vector3 &scale, 36 | const geometry_msgs::Pose &pose, 37 | const std::string &ns, 38 | const int32_t &id, 39 | const ColorRGBA &color, 40 | const std::string &frame_id, 41 | const int32_t &type); 42 | 43 | static Marker newCubeList(double scale, 44 | const std::string &ns, 45 | const int32_t &id, 46 | const ColorRGBA &color, 47 | const std::string &frame_id); 48 | 49 | static Marker newSphereList(const double &scale, 50 | const std::string &ns, 51 | const int32_t &id, 52 | const ColorRGBA &color, 53 | const std::string &frame_id); 54 | 55 | static Marker newLineStrip(const double &scale, 56 | const std::string &ns, 57 | const int32_t &id, 58 | const ColorRGBA &color, 59 | const std::string &frame_id); 60 | 61 | static Marker newLineList(const double &scale, 62 | const std::string &ns, 63 | const int32_t &id, 64 | const ColorRGBA &color, 65 | const std::string &frame_id); 66 | 67 | static Marker newCylinder(const geometry_msgs::Vector3 &scale, 68 | const geometry_msgs::Pose &pose, 69 | const std::string &ns, 70 | const int32_t &id, 71 | const ColorRGBA &color, 72 | const std::string &frame_id); 73 | 74 | static Marker newCube(const double &scale, 75 | const geometry_msgs::Pose &pose, 76 | const std::string &ns, 77 | const int32_t &id, 78 | const ColorRGBA &color, 79 | const std::string &frame_id); 80 | 81 | static Marker newSphere(const double &scale, 82 | const geometry_msgs::Pose &pose, 83 | const std::string &ns, 84 | const int32_t &id, 85 | const ColorRGBA &color, 86 | const std::string &frame_id); 87 | 88 | static Marker newArrow(const geometry_msgs::Vector3 &scale, 89 | const geometry_msgs::Pose &pose, 90 | const std::string &ns, 91 | const int32_t &id, 92 | const ColorRGBA &color, 93 | const std::string &frame_id); 94 | 95 | static Marker newText(const double &scale, 96 | const geometry_msgs::Pose &pose, 97 | const std::string &ns, 98 | const int32_t &id, 99 | const ColorRGBA &color, 100 | const std::string &frame_id); 101 | 102 | static Marker newFrame(const double &width, 103 | const double &length, 104 | const geometry_msgs::Pose &pose, 105 | const std::string &ns, 106 | const int32_t &id, 107 | const std::string &frame_id); 108 | 109 | static geometry_msgs::Pose defaultPose(); 110 | 111 | private: 112 | void initPublisher(); 113 | 114 | private: 115 | ros::NodeHandle nh; 116 | ros::Publisher rviz_pub; 117 | std::string topic; 118 | MarkerArray rviz_marker_array; 119 | }; 120 | } // namespace ros_viz_tools 121 | #endif //ROS_VIZ_TOOLS_H 122 | -------------------------------------------------------------------------------- /launch/demo_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_viz_tools 4 | 0.0.1 5 | The ros_viz_tools package 6 | 7 | 8 | 9 | 10 | Wei Wang 11 | 12 | 13 | 14 | 15 | MIT 16 | 17 | catkin 18 | geometry_msgs 19 | roscpp 20 | std_msgs 21 | visualization_msgs 22 | tf2 23 | 24 | -------------------------------------------------------------------------------- /src/color.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2019 Wei Wang (wei.wang.bit@outlook.com) 2 | 3 | #include "ros_viz_tools/color.h" 4 | namespace ros_viz_tools { 5 | ColorRGBA newColorRGBA(uint8_t red, uint8_t green, uint8_t blue, double alpha) { 6 | std_msgs::ColorRGBA color; 7 | color.r = red / 255.0; 8 | color.g = green / 255.0; 9 | color.b = blue / 255.0; 10 | color.a = alpha; 11 | return newColorRGBADouble(red / 255.0, green / 255.0, blue / 255.0, alpha); 12 | } 13 | 14 | ColorRGBA newColorRGBADouble(double red, double green, double blue, double alpha) { 15 | std_msgs::ColorRGBA color; 16 | color.r = red; 17 | color.g = green; 18 | color.b = blue; 19 | color.a = alpha; 20 | return color; 21 | } 22 | 23 | ColorMap::ColorMap(const std::vector &color_list) { 24 | if (color_list.size() < 2) { 25 | throw std::invalid_argument("ColorMap: size of color_list should be at least 2."); 26 | } 27 | color_list_ = color_list; 28 | double value_step = 1.0 / color_list_.size(); 29 | value_list_.clear(); 30 | for (unsigned long i = 0; i < color_list_.size(); i++) { 31 | value_list_.push_back(value_step * static_cast(i)); 32 | } 33 | value_list_.back() = 1.0; 34 | } 35 | 36 | size_t ColorMap::operator()(double value) { 37 | if (value < 0.0 || value > 1.0) { 38 | value = value < 0.0? 0.0:value; 39 | value = value > 1.0? 1.0:value; 40 | printf("ColorMap: value is out of range [0, 1], modified to %.1f.\n", value); 41 | } 42 | 43 | unsigned long index = 0; 44 | for (unsigned long i = 0; i < value_list_.size(); ++i) { 45 | if (value <= value_list_.at(i) && i != 0) { 46 | index = i - 1; 47 | } 48 | } 49 | double scale = (value - value_list_.at(index)) / (value_list_.at(index + 1) - value_list_.at(index)); 50 | 51 | return colorInterpolation(color_list_.at(index), color_list_.at(index + 1), scale);; 52 | } 53 | 54 | size_t ColorMap::colorRGB2Hex(uint8_t red, uint8_t green, uint8_t blue) { 55 | return static_cast((red << 16) + (green << 8) + blue); 56 | } 57 | 58 | void ColorMap::colorHex2RGB(const size_t &color_hex, uint8_t &red, uint8_t &green, uint8_t &blue) { 59 | red = color_hex >> 16 & 0xFF; 60 | green = color_hex >> 8 & 0xFF; 61 | blue = color_hex & 0xFF; 62 | } 63 | 64 | size_t ColorMap::colorInterpolation(size_t start, size_t end, double scale){ 65 | uint8_t red1, green1, blue1, red2, green2, blue2; 66 | colorHex2RGB(start, red1, green1, blue1); 67 | colorHex2RGB(end, red2, green2, blue2); 68 | auto red_at_value = static_cast(round((1 - scale) * red1 + scale * red2)); 69 | auto green_at_value = static_cast(round((1 - scale) * green1 + scale * green2)); 70 | auto blue_at_value = static_cast(round((1 - scale) * blue1 + scale * blue2)); 71 | return colorRGB2Hex(red_at_value, green_at_value, blue_at_value); 72 | } 73 | 74 | void ColorMap::linspaceColorRGBinHex(size_t start, size_t end, size_t num, std::vector &color_list) { 75 | if(!color_list.empty()){ 76 | color_list.clear(); 77 | } 78 | double scale; 79 | double scale_step = 1.0 / (num - 1); 80 | uint8_t red1, green1, blue1, red2, green2, blue2; 81 | colorHex2RGB(start, red1, green1, blue1); 82 | colorHex2RGB(end, red2, green2, blue2); 83 | for (size_t i = 0; i < num; ++i) { 84 | scale = i * scale_step; 85 | auto red_at_value = static_cast(round((1 - scale) * red1 + scale * red2)); 86 | auto green_at_value = static_cast(round((1 - scale) * green1 + scale * green2)); 87 | auto blue_at_value = static_cast(round((1 - scale) * blue1 + scale * blue2)); 88 | color_list.push_back(colorRGB2Hex(red_at_value, green_at_value, blue_at_value)); 89 | } 90 | } 91 | 92 | } // namespace ros_viz_tools 93 | -------------------------------------------------------------------------------- /src/demo_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2019 Wei Wang (wei.wang.bit@outlook.com) 2 | 3 | #include "ros_viz_tools/ros_viz_tools.h" 4 | #include 5 | 6 | static std::default_random_engine e; 7 | std::uniform_int_distribution randRGB(0, 255); 8 | 9 | using ros_viz_tools::RosVizTools; 10 | using ros_viz_tools::ColorMap; 11 | 12 | int main( int argc, char** argv ) 13 | { 14 | ROS_INFO("demo_node starts."); 15 | ros::init(argc, argv, "demo_node"); 16 | ros::NodeHandle n; 17 | std::string topic = "demo_marker"; 18 | ros_viz_tools::RosVizTools markers(n, topic); 19 | std::string frame_id = "ros_viz_tools"; 20 | std::string ns; 21 | 22 | ros::Rate r(1); 23 | while (ros::ok()) 24 | { 25 | markers.clear(); 26 | 27 | // Frame (Axes) 28 | visualization_msgs::Marker marker_frame1, marker_frame2; 29 | ns = "axes"; 30 | geometry_msgs::Pose pose; 31 | pose.position.x = -3.0; 32 | pose.position.y = 1.0; 33 | pose.position.z = -1.0; 34 | pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 45 * M_PI / 180, 45 * M_PI / 180)); 35 | marker_frame1 = RosVizTools::newFrame(0.1, 2.0, pose, ns, 0, frame_id); 36 | pose.position.x = -5.0; 37 | pose.position.y = 2.0; 38 | pose.position.z = -3.0; 39 | pose.orientation = tf2::toMsg(tf2::Quaternion(30 * M_PI / 180, 30 * M_PI / 180, 30 * M_PI / 180)); 40 | marker_frame2 = RosVizTools::newFrame(0.1, 1.0, pose, ns, 1, frame_id); 41 | markers.append(marker_frame1); 42 | markers.append(marker_frame2); 43 | 44 | // Cube List 45 | ns = "cube_list"; 46 | visualization_msgs::Marker marker_cubelist = RosVizTools::newCubeList(0.5, ns, 0, ros_viz_tools::WHITE, frame_id); 47 | for (int i = 0; i < 10; ++i) { 48 | geometry_msgs::Point p; 49 | p.x = i; 50 | p.y = pow(p.x, 2.0); 51 | p.z = 1.0; 52 | marker_cubelist.points.push_back(p); 53 | std_msgs::ColorRGBA color = ros_viz_tools::newColorRGBA(randRGB(e), randRGB(e), randRGB(e)); 54 | marker_cubelist.colors.push_back(color); 55 | } 56 | markers.append(marker_cubelist); 57 | 58 | // Line Strip 59 | ns = "line_strip"; 60 | visualization_msgs::Marker marker_linestrip = RosVizTools::newLineStrip(0.3, ns, 0, ros_viz_tools::LIGHT_BLUE, frame_id); 61 | for (int i = 0; i < 10; ++i) { 62 | geometry_msgs::Point p; 63 | p.x = i; 64 | p.y = pow(p.x, 2.0); 65 | p.z = 2.0; 66 | marker_linestrip.points.push_back(p); 67 | std_msgs::ColorRGBA color = ros_viz_tools::newColorRGBA(randRGB(e), randRGB(e), randRGB(e)); 68 | marker_linestrip.colors.push_back(color); 69 | } 70 | markers.append(marker_linestrip); 71 | 72 | // Sphere List 73 | ns = "sphere_list"; 74 | visualization_msgs::Marker marker_spherelist = RosVizTools::newSphereList(1.0, ns, 0, ros_viz_tools::LIME_GREEN, frame_id); 75 | for (int i = 0; i < 10; ++i) { 76 | geometry_msgs::Point p; 77 | p.x = i; 78 | p.y = pow(p.x, 2.0); 79 | p.z = 3.5; 80 | marker_spherelist.points.push_back(p); 81 | std_msgs::ColorRGBA color = ros_viz_tools::newColorRGBA(randRGB(e), randRGB(e), randRGB(e)); 82 | marker_spherelist.colors.push_back(color); 83 | } 84 | markers.append(marker_spherelist); 85 | 86 | // Text 87 | ns = "text"; 88 | pose.position.x = -2.0; 89 | pose.position.y = 2.0; 90 | pose.position.z = 2.0; 91 | pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 0 * M_PI / 180, 45 * M_PI / 180)); 92 | visualization_msgs::Marker marker_text = RosVizTools::newText(1.0, pose, ns, 0, ros_viz_tools::WHITE, frame_id); 93 | marker_text.text = "This is text marker."; 94 | markers.append(marker_text); 95 | 96 | // Cylinder 97 | ns = "cylinder"; 98 | geometry_msgs::Vector3 scale; 99 | scale.x = 0.5; 100 | scale.y = 0.5; 101 | scale.z = 1.0; 102 | pose.position.x = -2.0; 103 | pose.position.y = -2.0; 104 | pose.position.z = -2.0; 105 | pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 45 * M_PI / 180, 45 * M_PI / 180)); 106 | visualization_msgs::Marker marker_cylinder = RosVizTools::newCylinder(scale, pose , ns, 0, ros_viz_tools::WHITE, frame_id); 107 | markers.append(marker_cylinder); 108 | 109 | // Cube 110 | ns = "cube"; 111 | pose.position.x = -1.0; 112 | pose.position.y = -1.0; 113 | pose.position.z = -1.0; 114 | pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 45 * M_PI / 180, 45 * M_PI / 180)); 115 | visualization_msgs::Marker marker_cube = RosVizTools::newCube(1.0, pose , ns, 0, ros_viz_tools::WHITE, frame_id); 116 | markers.append(marker_cube); 117 | 118 | // Cube 119 | ns = "sphere"; 120 | pose.position.x = -3.0; 121 | pose.position.y = -3.0; 122 | pose.position.z = -3.0; 123 | pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 45 * M_PI / 180, 45 * M_PI / 180)); 124 | visualization_msgs::Marker marker_sphere = RosVizTools::newSphere(0.5, pose , ns, 0, ros_viz_tools::RED, frame_id); 125 | markers.append(marker_sphere); 126 | 127 | // Arrow 128 | ns = "arrow"; 129 | scale.x = 1.0; 130 | scale.y = 0.1; 131 | scale.z = 0.1; 132 | pose.position.x = 0.0; 133 | pose.position.y = 0.0; 134 | pose.position.z = 0.0; 135 | pose.orientation = tf2::toMsg(tf2::Quaternion(0 * M_PI / 180, 0 * M_PI / 180, 90 * M_PI / 180)); 136 | visualization_msgs::Marker marker_arrow = RosVizTools::newArrow(scale, pose , ns, 0, ros_viz_tools::WHITE, frame_id); 137 | markers.append(marker_arrow); 138 | 139 | // ColorMap 140 | size_t start_color = ColorMap::colorRGB2Hex(255, 0, 0); 141 | size_t end_color = ColorMap::colorRGB2Hex(0, 255, 0); 142 | std::vector color_list1, color_list2; 143 | ColorMap::linspaceColorRGBinHex(start_color, end_color, 2, color_list1); 144 | ColorMap colormap1(color_list1); 145 | color_list2.push_back(ColorMap::colorRGB2Hex(255, 0, 0)); 146 | color_list2.push_back(ColorMap::colorRGB2Hex(0, 255, 0)); 147 | ColorMap colormap2(color_list2); 148 | ns = "colormap"; 149 | visualization_msgs::Marker marker_colormap1 = RosVizTools::newLineStrip(0.2, ns, 0, ros_viz_tools::LIGHT_BLUE, frame_id); 150 | visualization_msgs::Marker marker_colormap2 = RosVizTools::newSphereList(0.2, ns, 1, ros_viz_tools::LIGHT_BLUE, frame_id); 151 | for (int i = 0; i < 20; ++i) { 152 | geometry_msgs::Point p; 153 | p.x = i * 0.5; 154 | p.y = sin(p.x); 155 | p.z = -1.0; 156 | auto scale_value = (p.y + 1.0) / 2.0; 157 | marker_colormap1.points.push_back(p); 158 | uint8_t red, green, blue; 159 | ColorMap::colorHex2RGB(colormap1(scale_value), red, green, blue); 160 | std_msgs::ColorRGBA color = ros_viz_tools::newColorRGBA(red, green, blue); 161 | marker_colormap1.colors.push_back(color); 162 | p.z = -2.0; 163 | marker_colormap2.points.push_back(p); 164 | ColorMap::colorHex2RGB(colormap2(scale_value), red, green, blue); 165 | color = ros_viz_tools::newColorRGBA(red, green, blue); 166 | marker_colormap2.colors.push_back(color); 167 | 168 | } 169 | markers.append(marker_colormap1); 170 | markers.append(marker_colormap2); 171 | 172 | // publish 173 | markers.publish(); 174 | 175 | r.sleep(); 176 | } 177 | } -------------------------------------------------------------------------------- /src/ros_viz_tools.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2019 Wei Wang (wei.wang.bit@outlook.com) 2 | 3 | #include "ros_viz_tools/ros_viz_tools.h" 4 | namespace ros_viz_tools { 5 | RosVizTools::RosVizTools(const ros::NodeHandle &nh, const std::string &topic) : 6 | nh(nh), topic(topic) { 7 | initPublisher(); 8 | } 9 | 10 | void RosVizTools::initPublisher() { 11 | rviz_pub = this->nh.advertise(topic, 1); 12 | } 13 | 14 | void RosVizTools::publish() { 15 | rviz_pub.publish(this->rviz_marker_array); 16 | } 17 | 18 | void RosVizTools::clear() { 19 | this->rviz_marker_array.markers.clear(); 20 | } 21 | 22 | void RosVizTools::append(const Marker &marker) { 23 | this->rviz_marker_array.markers.push_back(marker); 24 | } 25 | 26 | Marker RosVizTools::newMaker(const geometry_msgs::Vector3 &scale, 27 | const geometry_msgs::Pose &pose, 28 | const std::string &ns, 29 | const int32_t &id, 30 | const ColorRGBA &color, 31 | const std::string &frame_id, 32 | const int32_t &type) { 33 | Marker marker; 34 | // Set marker frame and timestamp. 35 | marker.header.frame_id = frame_id; 36 | marker.header.stamp = ros::Time::now(); 37 | 38 | // Set the namespace and id for this marker. 39 | marker.ns = ns; 40 | marker.id = id; 41 | 42 | // Set the marker type. 43 | marker.type = type; 44 | 45 | // Set the marker action. 46 | marker.action = visualization_msgs::Marker::ADD; 47 | 48 | // Set the pose of the marker. 49 | marker.pose = pose; 50 | 51 | // Set the scale and color of the marker. 52 | marker.scale = scale; 53 | marker.color = color; 54 | return marker; 55 | } 56 | 57 | Marker RosVizTools::newCubeList(double scale, 58 | const std::string &ns, 59 | const int32_t &id, 60 | const ColorRGBA &color, 61 | const std::string &frame_id) { 62 | geometry_msgs::Vector3 vec_scale; 63 | vec_scale.x = scale; 64 | vec_scale.y = scale; 65 | vec_scale.z = scale; 66 | return newMaker(vec_scale, defaultPose(), ns, id, color, frame_id, visualization_msgs::Marker::CUBE_LIST); 67 | } 68 | 69 | Marker RosVizTools::newSphereList(const double &scale, 70 | const std::string &ns, 71 | const int32_t &id, 72 | const ColorRGBA &color, 73 | const std::string &frame_id) { 74 | geometry_msgs::Vector3 vec_scale; 75 | vec_scale.x = scale; 76 | vec_scale.y = scale; 77 | vec_scale.z = scale; 78 | return newMaker(vec_scale, defaultPose(), ns, id, color, frame_id, visualization_msgs::Marker::SPHERE_LIST); 79 | } 80 | 81 | Marker RosVizTools::newLineStrip(const double &scale, 82 | const std::string &ns, 83 | const int32_t &id, 84 | const ColorRGBA &color, 85 | const std::string &frame_id) { 86 | geometry_msgs::Vector3 vec_scale; 87 | vec_scale.x = scale; 88 | vec_scale.y = 1.0; 89 | vec_scale.z = 1.0; 90 | return newMaker(vec_scale, defaultPose(), ns, id, color, frame_id, visualization_msgs::Marker::LINE_STRIP); 91 | } 92 | 93 | Marker RosVizTools::newLineList(const double &scale, 94 | const std::string &ns, 95 | const int32_t &id, 96 | const ColorRGBA &color, 97 | const std::string &frame_id) { 98 | geometry_msgs::Vector3 vec_scale; 99 | vec_scale.x = scale; 100 | vec_scale.y = 1.0; 101 | vec_scale.z = 1.0; 102 | return newMaker(vec_scale, defaultPose(), ns, id, color, frame_id, visualization_msgs::Marker::LINE_LIST); 103 | } 104 | 105 | Marker RosVizTools::newCylinder(const geometry_msgs::Vector3 &scale, 106 | const geometry_msgs::Pose &pose, 107 | const std::string &ns, 108 | const int32_t &id, 109 | const ColorRGBA &color, 110 | const std::string &frame_id) { 111 | return newMaker(scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::CYLINDER); 112 | } 113 | 114 | Marker RosVizTools::newCube(const double &scale, 115 | const geometry_msgs::Pose &pose, 116 | const std::string &ns, 117 | const int32_t &id, 118 | const ColorRGBA &color, 119 | const std::string &frame_id) { 120 | geometry_msgs::Vector3 vec_scale; 121 | vec_scale.x = scale; 122 | vec_scale.y = scale; 123 | vec_scale.z = scale; 124 | return newMaker(vec_scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::CUBE); 125 | } 126 | 127 | Marker RosVizTools::newSphere(const double &scale, 128 | const geometry_msgs::Pose &pose, 129 | const std::string &ns, 130 | const int32_t &id, 131 | const ColorRGBA &color, 132 | const std::string &frame_id) { 133 | geometry_msgs::Vector3 vec_scale; 134 | vec_scale.x = scale; 135 | vec_scale.y = scale; 136 | vec_scale.z = scale; 137 | return newMaker(vec_scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::SPHERE); 138 | } 139 | 140 | Marker RosVizTools::newArrow(const geometry_msgs::Vector3 &scale, 141 | const geometry_msgs::Pose &pose, 142 | const std::string &ns, 143 | const int32_t &id, 144 | const ColorRGBA &color, 145 | const std::string &frame_id) { 146 | return newMaker(scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::ARROW); 147 | } 148 | 149 | Marker RosVizTools::newText(const double &scale, 150 | const geometry_msgs::Pose &pose, 151 | const std::string &ns, 152 | const int32_t &id, 153 | const ColorRGBA &color, 154 | const std::string &frame_id) { 155 | geometry_msgs::Vector3 vec_scale; 156 | vec_scale.x = 1.0; 157 | vec_scale.y = 1.0; 158 | vec_scale.z = scale; 159 | return newMaker(vec_scale, pose, ns, id, color, frame_id, visualization_msgs::Marker::TEXT_VIEW_FACING); 160 | } 161 | 162 | Marker RosVizTools::newFrame(const double &width, 163 | const double &length, 164 | const geometry_msgs::Pose &pose, 165 | const std::string &ns, 166 | const int32_t &id, 167 | const std::string &frame_id) { 168 | 169 | // line list marker 170 | Marker frame = newLineList(width, ns, id, ros_viz_tools::WHITE, frame_id); 171 | 172 | // transform matrix - world origin to frame origin 173 | tf2::Transform trans_world_ori; 174 | tf2::convert(pose, trans_world_ori); 175 | 176 | // transform matrix - frame origin to key point of x, y, z axes 177 | tf2::Matrix3x3 mat(tf2::Quaternion(0, 0, 0, 1)); 178 | tf2::Vector3 pos_x(length, 0, 0); 179 | tf2::Vector3 pos_y(0, length, 0); 180 | tf2::Vector3 pos_z(0, 0, length); 181 | tf2::Transform trans_ori_x(mat, pos_x); 182 | tf2::Transform trans_ori_y(mat, pos_y); 183 | tf2::Transform trans_ori_z(mat, pos_z); 184 | 185 | // transform matrix - world origin to key point of x, y, z axes 186 | tf2::Transform trans_world_x = trans_world_ori * trans_ori_x; 187 | tf2::Transform trans_world_y = trans_world_ori * trans_ori_y; 188 | tf2::Transform trans_world_z = trans_world_ori * trans_ori_z; 189 | 190 | geometry_msgs::Point p; 191 | 192 | // x axis 193 | p.x = pose.position.x; 194 | p.y = pose.position.y; 195 | p.z = pose.position.z; 196 | frame.points.push_back(p); 197 | frame.colors.push_back(RED); 198 | tf2::Vector3 r_wx = trans_world_x.getOrigin(); 199 | p.x = r_wx[0]; 200 | p.y = r_wx[1]; 201 | p.z = r_wx[2]; 202 | frame.points.push_back(p); 203 | frame.colors.push_back(RED); 204 | 205 | // y axis 206 | p.x = pose.position.x; 207 | p.y = pose.position.y; 208 | p.z = pose.position.z; 209 | frame.points.push_back(p); 210 | frame.colors.push_back(GREEN); 211 | tf2::Vector3 r_wy = trans_world_y.getOrigin(); 212 | p.x = r_wy[0]; 213 | p.y = r_wy[1]; 214 | p.z = r_wy[2]; 215 | frame.points.push_back(p); 216 | frame.colors.push_back(GREEN); 217 | 218 | // z axis 219 | p.x = pose.position.x; 220 | p.y = pose.position.y; 221 | p.z = pose.position.z; 222 | frame.points.push_back(p); 223 | frame.colors.push_back(BLUE); 224 | tf2::Vector3 r_wz = trans_world_z.getOrigin(); 225 | p.x = r_wz[0]; 226 | p.y = r_wz[1]; 227 | p.z = r_wz[2]; 228 | frame.points.push_back(p); 229 | frame.colors.push_back(BLUE); 230 | 231 | return frame; 232 | } 233 | 234 | geometry_msgs::Pose RosVizTools::defaultPose() { 235 | geometry_msgs::Pose pose; 236 | pose.position.x = 0.0; 237 | pose.position.y = 0.0; 238 | pose.position.z = 0.0; 239 | pose.orientation.x = 0.0; 240 | pose.orientation.y = 0.0; 241 | pose.orientation.z = 0.0; 242 | pose.orientation.w = 1.0; 243 | return pose; 244 | } 245 | } // namespace ros_viz_tools --------------------------------------------------------------------------------