├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include └── ompl_visual_tools │ ├── common_utils.h │ ├── costs │ └── cost_map_2d_optimization_objective.h │ ├── moveit_viz_window.h │ ├── projection_viz_window.h │ ├── ros_viz_window.h │ └── utilities │ └── ppm.h ├── launch ├── ompl_rviz.launch └── ompl_rviz.rviz ├── package.xml ├── plot ├── plot.pg ├── run_plot.sh └── temperatures.dat ├── resources ├── grand_canyon.gif ├── grand_canyon.ppm ├── height_map0.png ├── height_map0.ppm ├── height_map1.jpg ├── height_map1.ppm ├── height_map2.jpg ├── height_map2.ppm └── wilbur_medium │ ├── wilbur_medium1.ppm │ └── wilbur_medium2.ppm ├── screenshots ├── ompl_visual_tools.png ├── repaired_path.png └── similar_paths.png └── src ├── demos └── rrt_demo.cpp ├── moveit_viz_window.cpp ├── projection_viz_window.cpp ├── ros_viz_window.cpp └── utilities └── ppm.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | \#*# 3 | *~ 4 | ~* 5 | Debug/ 6 | Release/ 7 | build/ 8 | bin/ 9 | CMakeCache.txt 10 | *.dat 11 | 12 | 13 | 14 | resources/ 15 | 16 | CATKIN_IGNORE -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci/ package. 2 | sudo: required 3 | dist: trusty 4 | services: 5 | - docker 6 | language: generic 7 | compiler: 8 | - gcc 9 | notifications: 10 | email: 11 | recipients: 12 | - dave@dav.ee 13 | env: 14 | matrix: 15 | - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-planning/moveit_docs/kinetic-devel/moveit.rosinstall 16 | - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-planning/moveit_docs/kinetic-devel/moveit.rosinstall 17 | matrix: 18 | allow_failures: 19 | - env: ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-planning/moveit_docs/kinetic-devel/moveit.rosinstall 20 | before_script: 21 | - git clone -q https://github.com/ros-planning/moveit_ci.git .moveit_ci 22 | script: 23 | - source .moveit_ci/travis.sh -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ompl_visual_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 2.3.2 (2016-01-13) 6 | ------------------ 7 | * Removed usage of publishSamples in demo 8 | * Updated README 9 | * Added travis support 10 | * Contributors: Dave Coleman 11 | 12 | 2.3.1 (2015-12-07) 13 | ------------------ 14 | * catkin lint cleanup 15 | * Contributors: Dave Coleman 16 | 17 | 2.3.0 (2015-12-05) 18 | ------------------ 19 | * Fixed API changes in rviz_visual_tools 20 | * Contributors: Dave Coleman 21 | 22 | 2.2.1 (2015-01-07) 23 | ------------------ 24 | * Fix typo 25 | * Added missing images 26 | * Fix install space 27 | * Contributors: Dave Coleman 28 | 29 | 2.2.0 (2014-10-31) 30 | ------------------ 31 | * Fix for RvizVisualTools 32 | * Upgrade to new moveit_visual_tools API 33 | * API changes for moveit_visual_tools 34 | * Added gitignore 35 | * New publishState() functions 36 | * New publishRobotState function 37 | * Formatting, new callback parameter 38 | * Deprecated publishSamples functions 39 | * Added publishSpheres functions to correspond to moveit_visual_tools ones 40 | * Improved visualizationStateCallback 41 | * New publishRobotGraph function 42 | * New convertRobotStatesToTipPoints function 43 | * Fixing display database mode 44 | * Made publish functions return bool 45 | * Disable 3D option 46 | * publish cost map with non-static ID numbers 47 | * New publishSampleIDs function. Restructured publishSamples interface to use moveit_visual_tools version 48 | * Updated publishSampels() functions 49 | * Reduced size of published spheres 50 | * Contributors: Dave Coleman 51 | 52 | 2.1.1 (2014-08-11) 53 | ------------------ 54 | * Removed debug output 55 | * Removed hard-coded base_frame name 56 | * Improved memory usage 57 | * Cost ptr bug fix 58 | * Removed setOptimizationMethod() 59 | * Contributors: Dave Coleman 60 | 61 | 2.1.0 (2014-08-08) 62 | ------------------ 63 | * Removed getSolutionPlannerName that is not available in OMPL released version 64 | * Updated README 65 | * Renamed file to rrt_demo.cpp 66 | * Cleanup since change to moveit_visual_tools 67 | * Specify OMPL version 68 | * Fixed marker topic path 69 | * Contributors: Dave Coleman 70 | 71 | 2.0.1 (2014-08-07) 72 | ------------------ 73 | * Updated README 74 | * Removed lightning dependencies 75 | * Renamed ompl_rviz_viewer to ompl_visual_tools 76 | * Initial 77 | * Contributors: Dave Coleman 78 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ompl_visual_tools) 3 | 4 | # C++ 11 5 | add_compile_options(-std=c++11) 6 | 7 | # Load catkin and all dependencies required for this package 8 | find_package(catkin REQUIRED COMPONENTS 9 | graph_msgs 10 | moveit_visual_tools 11 | moveit_core 12 | moveit_ompl 13 | roscpp 14 | roslib 15 | visualization_msgs 16 | ) 17 | 18 | find_package(ompl REQUIRED) 19 | find_package(Boost) 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS 23 | graph_msgs 24 | moveit_visual_tools 25 | moveit_core 26 | moveit_ompl 27 | roscpp 28 | roslib 29 | visualization_msgs 30 | INCLUDE_DIRS 31 | include 32 | LIBRARIES 33 | ${PROJECT_NAME} 34 | ${PROJECT_NAME}_ppm 35 | ) 36 | 37 | ########### 38 | ## Build ## 39 | ########### 40 | 41 | include_directories(SYSTEM 42 | ${Boost_INCLUDE_DIRS} 43 | ${OMPL_INCLUDE_DIRS} 44 | ) 45 | 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${OMPL_INCLUDE_DIRS} 50 | ) 51 | 52 | # PPM Library for loading images 53 | add_library(${PROJECT_NAME}_ppm 54 | src/utilities/ppm.cpp 55 | ) 56 | target_link_libraries(${PROJECT_NAME}_ppm 57 | ${catkin_LIBRARIES} ${Boost_LIBRARIES} 58 | ) 59 | 60 | # OMPL Visual Tools Library 61 | add_library(${PROJECT_NAME} 62 | src/ros_viz_window.cpp 63 | src/moveit_viz_window.cpp 64 | src/projection_viz_window.cpp 65 | ) 66 | target_link_libraries(${PROJECT_NAME} 67 | ${PROJECT_NAME}_ppm 68 | ${catkin_LIBRARIES} 69 | ${Boost_LIBRARIES} 70 | ) 71 | 72 | # Executable for Demos and Testing 73 | # add_executable(${PROJECT_NAME}_rrt_demo 74 | # src/demos/rrt_demo.cpp 75 | # ) 76 | # set_target_properties(${PROJECT_NAME}_rrt_demo 77 | # PROPERTIES OUTPUT_NAME rrt_demo PREFIX "" 78 | # ) 79 | # target_link_libraries(${PROJECT_NAME}_rrt_demo 80 | # ${OMPL_LIBRARIES} 81 | # ${catkin_LIBRARIES} 82 | # ${Boost_LIBRARIES} 83 | # ${PROJECT_NAME} 84 | # ${PROJECT_NAME}_ppm 85 | # ) 86 | 87 | ## Install 88 | 89 | # Install libraries 90 | install(TARGETS ${PROJECT_NAME}_ppm ${PROJECT_NAME} 91 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 92 | 93 | # Install header files 94 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 95 | 96 | # Install shared resources 97 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 98 | install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 99 | 100 | # # Install executables 101 | # install(TARGETS 102 | # ${PROJECT_NAME}_rrt_demo 103 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 104 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 105 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OMPL Visual Tools 2 | 3 | # THIS LIBRAY IS NO LONGER MAINTAINED IN KINETIC ONWARD 4 | 5 | The configuration with a custom version of OMPL was too much to maintain for me, and I am stopping support for this package as of ROS Kinetic. 6 | 7 | --- 8 | 9 | The OMPL Visual Tools is a library for visualizing and debugging [Open Motion Planning Library](http://ompl.kavrakilab.org/) algorithms in Rviz. 10 | 11 | Formally named ompl_rviz_viewer until August 2014, this project spun out of an internship at Willow Garage. [Video](https://www.youtube.com/watch?v=RcGvi4Svd4k) 12 | 13 | The ompl_visual_tools was originally develped for testing cost-based algorithms in a two dimensional space with a third dimension displayed as cost. 14 | The space is specified as a grey scale cost map image that can be passed in to the program. 15 | The lighter (closer to white) each pixel of the image is, the "higher the cost" is considered to be. Black is considered no cost. 16 | Additionally, absolute obstacles can be specified by defining a max limit to the cost, such that any value above that threshold is considered an obstacle. 17 | 18 | This little program is similar to the OMPL.app that is distributed with OMPL, but instead uses RViz for visualization and is more 19 | streamlined for considering costs and experience-based planning. 20 | 21 | Developed by [Dave Coleman](http://dav.ee/) at the University of Colorado Boulder and Willow Garage 22 | 23 | Status: 24 | 25 | * [![Build Status](https://travis-ci.org/davetcoleman/ompl_visual_tools.svg)](https://travis-ci.org/davetcoleman/ompl_visual_tools) Travis CI 26 | * [![Devel Job Status](http://jenkins.ros.org/buildStatus/icon?job=devel-indigo-ompl_visual_tools)](http://jenkins.ros.org/job/devel-indigo-ompl_visual_tools) Devel Job Status 27 | * [![Build Status](http://jenkins.ros.org/buildStatus/icon?job=ros-indigo-ompl-visual-tools_binarydeb_trusty_amd64)](http://jenkins.ros.org/job/ros-indigo-ompl-visual-tools_binarydeb_trusty_amd64/) AMD64 Debian Job Status 28 | 29 | ![](screenshots/ompl_visual_tools.png) 30 | 31 | ## Install 32 | 33 | ### Ubuntu Debian 34 | 35 | sudo apt-get install ros-indigo-ompl-visual-tools 36 | 37 | ### Build from Source 38 | 39 | To build this package, ``git clone`` this repo into a [catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) and be sure to install necessary dependencies by running the following command in the root of your catkin workspace: 40 | 41 | rosdep install -y --from-paths src --ignore-src --rosdistro indigo 42 | 43 | ## Code API 44 | 45 | See [Class Reference](http://docs.ros.org/indigo/api/my_package/html/) 46 | 47 | ## Usage 48 | 49 | This library can be integrated into your project to easily view a 2D, 3D or robot planning environment in Rviz. 50 | 51 | First, load the visualizer: 52 | 53 | ``` 54 | // The visual tools for interfacing with Rviz 55 | ompl_visual_tools::OmplVisualToolsPtr visual_tools_; 56 | 57 | // Load the tool for displaying in Rviz 58 | visual_tools_.reset(new ompl_visual_tools::OmplVisualTools(BASE_FRAME)); 59 | visual_tools_->setSpaceInformation(si_); 60 | visual_tools_->setGlobalScale(100); 61 | 62 | // Clear current rviz makers 63 | visual_tools_->deleteAllMarkers(); 64 | ``` 65 | 66 | ### Two Dimensions with optional cost map 67 | 68 | To test with a 2D environment with a cost map: 69 | ``` 70 | // Cost in 2D 71 | ompl::base::CostMap2DOptimizationObjectivePtr cost_map_; 72 | cost_map_->max_cost_threshold_percent_ = max_cost_threshold_percent; 73 | cost_map_->loadImage(image_path); 74 | 75 | // Pass cost to visualizer 76 | visual_tools_->setCostMap(cost_map_->cost_); 77 | ``` 78 | 79 | To view the cost map in Rviz: 80 | ``` 81 | visual_tools_->publishCostMap(cost_map_->image_); 82 | ``` 83 | 84 | To view the start and goal location: 85 | ``` 86 | visual_tools_->publishState(start, rviz_visual_tools::GREEN, rviz_visual_tools::XLARGE, "plan_start_goal"); 87 | visual_tools_->publishState(goal, rviz_visual_tools::ORANGE, rviz_visual_tools::XLARGE, "plan_start_goal"); 88 | ``` 89 | 90 | To view the solution path: 91 | ``` 92 | // Interpolate solution 93 | simple_setup_->getSolutionPath().interpolate(); 94 | 95 | // Show path 96 | visual_tools_->publishPath( simple_setup_->getSolutionPath(), rviz_visual_tools::GREEN, 1.0, "final_solution"); 97 | ``` 98 | 99 | And to see more of what the planner was doing: 100 | ``` 101 | // Visualize the explored space 102 | visual_tools_->publishGraph(planner_data, rviz_visual_tools::ORANGE, 0.2, "tree"); 103 | 104 | // Visualize the sample locations 105 | visual_tools_->publishSamples(planner_data); 106 | ``` 107 | 108 | ### MoveIt! Robot Planning 109 | 110 | See moveit_visual_tools for more information about tools this class can use with MoveIt!. For OMPL-specific features: 111 | 112 | First, set the state space that MoveIt! has chosen for your robot in OMPL: 113 | 114 | ``` 115 | // Create a state space describing our robot's planning group 116 | ompl_interface::ModelBasedStateSpaceSpecification model_ss_spec(moveit_robot_model, joint_model_group); 117 | const ompl_interface::JointModelStateSpaceFactory factory; 118 | ompl_interface::ModelBasedStateSpacePtr model_state_space = factory.getNewStateSpace(model_ss_spec); 119 | 120 | // Setup the state space 121 | model_state_space->setup(); 122 | 123 | visual_tools_->setStateSpace(model_state_space); 124 | ``` 125 | 126 | Then you can publish the paths of various tips on a robot, as planned in OMPL: 127 | ``` 128 | std::vector paths; 129 | simple_setup.getAllPlannerDatas(paths); 130 | 131 | // Get tip links for this setup 132 | std::vector tips; 133 | joint_model_group_->getEndEffectorTips(tips); 134 | 135 | bool show_trajectory_animated = true; 136 | visual_tools_->publishRobotPath(paths[0], joint_model_group, tips, show_trajectory_animated); 137 | ``` 138 | 139 | ## View in Rviz 140 | 141 | Start Rviz using the included launch file: 142 | 143 | ``` 144 | roslaunch ompl_visual_tools ompl_rviz.launch 145 | ``` 146 | 147 | ### Demo planner with standard RRT: 148 | 149 | ``` 150 | rosrun ompl_visual_tools rrt_demo 151 | ``` 152 | 153 | To see optional parameters, pass in ``--help`` argument. There are many options. 154 | 155 | **Note:** *To change the algorithm being used, manually edit the code* 156 | 157 | ## Cost Map Usage 158 | 159 | A default cost map image will be used, located in the resources/ folder, for running the algorithm. 160 | Optionally one can pass in their own cost map image through a command line argument, as shown in the following example: 161 | 162 | ``` 163 | rosrun ompl_visual_tools rrt_demo cost_map.ppm 164 | ``` 165 | 166 | The image must be in the PPM "Netpbm color image" format. To convert a jpg, png or any other image into this format on Linux simply use the "convert" command, as shown in the following example: 167 | 168 | ``` 169 | sudo apt-get install imagemagick 170 | convert cost_map.png cost_map.ppm 171 | ``` 172 | 173 | In general, a 100x100 pixel image is a decent space size, and larger dimensions will require much more computational resources or may hang your computer. GIMP is a good editor for scaling images down. 174 | 175 | ## TODO 176 | 177 | - Remove PPM library and instead depend on OMPL's version 178 | 179 | ## Develop 180 | 181 | You are encouraged to fork this package on GitHub and test your own cost-based planning algorithms using this visualizer! 182 | -------------------------------------------------------------------------------- /include/ompl_visual_tools/common_utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Functions shared by both ros_viz_window and moveit_viz_window 37 | */ 38 | 39 | #ifndef OMPL_VISUAL_TOOLS__COMMON_UTILS_ 40 | #define OMPL_VISUAL_TOOLS__COMMON_UTILS_ 41 | 42 | // Datatypes 43 | #include 44 | 45 | namespace ompl_visual_tools 46 | { 47 | 48 | 49 | 50 | } // namespace ompl_visual_tools 51 | 52 | #endif // OMPL_VISUAL_TOOLS__COMMON_UTILS_ 53 | -------------------------------------------------------------------------------- /include/ompl_visual_tools/costs/cost_map_2d_optimization_objective.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | * Desc: Optimization objective that simply reads a value from a 2D cost map 37 | */ 38 | 39 | 40 | #ifndef OMPL_VISUAL_TOOLS_COST_MAP_OPTIMIZATION_OBJECTIVE_H 41 | #define OMPL_VISUAL_TOOLS_COST_MAP_OPTIMIZATION_OBJECTIVE_H 42 | 43 | // OMPL 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // Boost 50 | #include 51 | #include 52 | 53 | // For reading image files 54 | #include 55 | 56 | namespace ompl_visual_tools 57 | { 58 | typedef boost::numeric::ublas::matrix intMatrix; 59 | typedef std::shared_ptr intMatrixPtr; 60 | } 61 | 62 | namespace ob = ompl::base; 63 | namespace og = ompl::geometric; 64 | 65 | namespace ompl 66 | { 67 | namespace base 68 | { 69 | /** \brief An optimization objective which defines path cost using the idea of mechanical work. */ 70 | class CostMap2DOptimizationObjective : public OptimizationObjective 71 | { 72 | public: 73 | /** \brief Constructor */ 74 | CostMap2DOptimizationObjective(const SpaceInformationPtr &si, double pathLengthWeight = 0.0001) 75 | : OptimizationObjective(si), 76 | image_(NULL), 77 | max_cost_threshold_percent_(0.4), 78 | pathLengthWeight_(pathLengthWeight) 79 | { 80 | description_ = "Cost Map"; 81 | 82 | cost_.reset(new ompl_visual_tools::intMatrix()); 83 | }; 84 | 85 | /** \brief Deconstructor */ 86 | ~CostMap2DOptimizationObjective() 87 | { 88 | delete image_; 89 | }; 90 | 91 | double getPathLengthWeight() const 92 | { 93 | return pathLengthWeight_; 94 | } 95 | 96 | /** \brief Defines motion cost */ 97 | virtual ompl::base::Cost motionCost(const State *s1, const State *s2) const 98 | { 99 | // Only accrue positive changes in cost 100 | double positiveCostAccrued = std::max(stateCost(s2).value() - stateCost(s1).value(), 0.0); 101 | return Cost(positiveCostAccrued + pathLengthWeight_*si_->distance(s1,s2)); 102 | }; 103 | 104 | ompl::base::Cost stateCost(const State *state) const 105 | { 106 | const double *coords = state->as()->values; 107 | 108 | // Return the cost from the matrix at the current dimensions 109 | double cost = (*cost_)( natRound(coords[1]), natRound(coords[0]) ); 110 | 111 | return Cost(cost); 112 | } 113 | 114 | /** \brief Passed in a cost matrix loaded from an image file, etc */ 115 | void setCostMatrix(ompl_visual_tools::intMatrixPtr cost) 116 | { 117 | cost_ = cost; 118 | }; 119 | 120 | /** 121 | * \brief NatRounding helper function to make readings from cost map more accurate 122 | * \param double 123 | * \return rounded down number 124 | */ 125 | int natRound(double x) const 126 | { 127 | return static_cast(floor(x + 0.5f)); 128 | }; 129 | 130 | void loadImage( std::string image_path ) 131 | { 132 | // Load cost map from image file 133 | image_ = ompl_visual_tools::readPPM( image_path.c_str() ); 134 | 135 | // Error check 136 | if( !image_ ) 137 | { 138 | ROS_ERROR( "No image data loaded " ); 139 | return; 140 | } 141 | 142 | // Disallow non-square 143 | if( image_->x != image_->y ) 144 | { 145 | ROS_ERROR( "Does not currently support non-square images because of some weird bug. Feel free to fork and fix!" ); 146 | return; 147 | } 148 | 149 | ROS_INFO_STREAM( "Map Height: " << image_->y << " Map Width: " << image_->x ); 150 | 151 | // Create an array of ints that represent the cost of every pixel 152 | cost_->resize( image_->x, image_->y ); 153 | 154 | // Generate the cost map 155 | createCostMap(); 156 | }; 157 | 158 | /** 159 | * \brief Helper Function: calculate cost map 160 | */ 161 | void createCostMap() 162 | { 163 | // gets the min and max values of the cost map 164 | getMinMaxPixels(); 165 | 166 | // This factor is the author's visual preference for scaling a cost map in Rviz 167 | const double artistic_scale = 6.0; // smaller is taller 168 | 169 | const double pixel_diff = max_pixel_ - min_pixel_; 170 | 171 | // This scale adapts that factor depending on the cost map min max 172 | const double scale = pixel_diff / ( image_->x / artistic_scale ); //image->x is width 173 | 174 | // Dynamically calculate the obstacle threshold 175 | max_cost_threshold_ = (max_pixel_ - ( max_cost_threshold_percent_ * pixel_diff )) / scale; 176 | 177 | // Preprocess the pixel data for cost and give it a nice colored tint 178 | for( size_t i = 0; i < image_->getSize(); ++i ) 179 | { 180 | // Calculate cost 181 | cost_->data()[i] = ( image_->data[ i ].red - min_pixel_ ) / scale; 182 | 183 | // Prevent cost from being zero 184 | if( !cost_->data()[i] ) 185 | cost_->data()[i] = 1; 186 | 187 | // Color different if it is an obstacle 188 | if( cost_->data()[i] >= max_cost_threshold_ || cost_->data()[i] <= 1) 189 | { 190 | //std::cout << "cost is " << cost_->data()[i] << " threshold is " << max_cost_threshold_ << std::endl; 191 | 192 | image_->data[ i ].red = 255; //image_->data[ i ].red; 193 | image_->data[ i ].green = image_->data[ i ].green; 194 | image_->data[ i ].blue = image_->data[ i ].blue; 195 | } 196 | 197 | } 198 | 199 | } 200 | 201 | /** 202 | * \brief Helper Function: gets the min and max values of the cost map 203 | */ 204 | void getMinMaxPixels() 205 | { 206 | // Find the min and max cost from the image 207 | min_pixel_ = image_->data[ 0 ].red; 208 | max_pixel_ = image_->data[ 0 ].red; 209 | 210 | for( size_t i = 0; i < image_->getSize(); ++i ) 211 | { 212 | // Max 213 | if( image_->data[ i ].red > max_pixel_ ) 214 | max_pixel_ = image_->data[ i ].red; 215 | // Min 216 | else if( image_->data[ i ].red < min_pixel_ ) 217 | min_pixel_ = image_->data[ i ].red; 218 | } 219 | 220 | // Override for blank images 221 | if (max_pixel_ < std::numeric_limits::epsilon()) 222 | { 223 | std::cout << "BLANK IMAGE - cost_map_2d_opt... " << std::endl; 224 | min_pixel_ = 0; 225 | max_pixel_ = 255; 226 | } 227 | } 228 | 229 | // The RGB image data 230 | ompl_visual_tools::PPMImage *image_; 231 | 232 | // The cost for each x,y - which is derived from the RGB data 233 | ompl_visual_tools::intMatrixPtr cost_; 234 | 235 | // The cost at which it becomes an obstacle 236 | double max_cost_threshold_; 237 | 238 | // The percentage of the top min/max cost value that is considered an obstacle, e.g. 0.1 is top 10% of peaks 239 | double max_cost_threshold_percent_; 240 | 241 | protected: 242 | 243 | /** \brief The weighing factor for the path length in the mechanical work objective formulation. */ 244 | double pathLengthWeight_; 245 | 246 | // Remember the min and max cost from the image 247 | int max_pixel_; 248 | int min_pixel_; 249 | 250 | }; 251 | 252 | typedef std::shared_ptr< CostMap2DOptimizationObjective > CostMap2DOptimizationObjectivePtr; 253 | } 254 | } 255 | 256 | #endif 257 | -------------------------------------------------------------------------------- /include/ompl_visual_tools/moveit_viz_window.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tools for displaying OMPL components in Rviz 37 | */ 38 | 39 | #ifndef OMPL_VISUAL_TOOLS__MOVEIT_VIZ_WINDOW_ 40 | #define OMPL_VISUAL_TOOLS__MOVEIT_VIZ_WINDOW_ 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | // OMPL 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | // MoveIt 55 | #include 56 | 57 | // Visualization 58 | #include 59 | 60 | namespace ob = ompl::base; 61 | namespace og = ompl::geometric; 62 | namespace rvt = rviz_visual_tools; 63 | 64 | namespace ompl_interface 65 | { 66 | class ModelBasedPlanningContext; 67 | typedef std::shared_ptr ModelBasedPlanningContextPtr; 68 | } 69 | 70 | namespace ompl_visual_tools 71 | { 72 | class MoveItVizWindow : public ompl::tools::VizWindow 73 | { 74 | public: 75 | MoveItVizWindow(moveit_visual_tools::MoveItVisualToolsPtr visuals, ompl::base::SpaceInformationPtr si); 76 | 77 | /** \brief Visualize a state during runtime, externally */ 78 | void state(const ompl::base::State* state, ompl::tools::VizSizes size, ompl::tools::VizColors color, 79 | double extraData); 80 | 81 | /** \brief Visualize multiple states during runtime, externally */ 82 | void states(std::vector states, std::vector colors, ompl::tools::VizSizes size); 83 | 84 | /** \brief Visualize edge during runtime, externally */ 85 | void edge(const ompl::base::State* stateA, const ompl::base::State* stateB, double cost); 86 | 87 | /** \brief Visualize edge with a level during runtime, externally */ 88 | void edge(const ompl::base::State* stateA, const ompl::base::State* stateB, ompl::tools::VizSizes size, 89 | ompl::tools::VizColors color); 90 | 91 | /** \brief Visualize multiple edges during runtime, externally */ 92 | // void edges(const std::vector stateAs, const std::vector stateBs, 93 | // std::vector colors, ompl::tools::VizSizes size){}; 94 | 95 | /** 96 | * \brief Publish a full path of multiple points and edges 97 | * \param path 98 | * \param type - the style to display the line as 99 | * \return true on success 100 | */ 101 | void path(ompl::geometric::PathGeometric* path, ompl::tools::VizSizes type, ompl::tools::VizColors color); 102 | 103 | /** \brief Trigger visualizer to refresh/repaint/display all graphics */ 104 | void trigger(); 105 | 106 | /** \brief Trigger visualizer to clear all graphics */ 107 | void deleteAllMarkers(); 108 | 109 | /** \brief Check if SIGINT has been called */ 110 | bool shutdownRequested(); 111 | 112 | /** \brief Get the underlying visualizer */ 113 | moveit_visual_tools::MoveItVisualToolsPtr getVisualTools() 114 | { 115 | return visuals_; 116 | } 117 | 118 | // From ompl_visual_tools ------------------------------------------------------ 119 | 120 | /** 121 | * \brief Publish a marker of a series of spheres to rviz 122 | * \param spheres - where to publish them 123 | * \param color - an enum pre-defined name of a color 124 | * \param scale - an enum pre-defined name of a size 125 | * \param ns - namespace of marker 126 | * \return true on success 127 | */ 128 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color = rviz_visual_tools::RED, 129 | double scale = 0.1, const std::string& ns = "path_spheres"); 130 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color = rviz_visual_tools::RED, 131 | const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, 132 | const std::string& ns = "path_spheres"); 133 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color, 134 | const geometry_msgs::Vector3& scale, const std::string& ns = "path_spheres"); 135 | 136 | /** 137 | * \brief Display States 138 | * \return true on success 139 | */ 140 | //bool publishStates(std::vector states); 141 | 142 | /** 143 | * \brief Convert an OMPL state to a MoveIt! robot state and publish it 144 | * \param OMPL format of a robot state 145 | * \return true on success 146 | */ 147 | bool publishRobotState(const ompl::base::State* state); 148 | 149 | /** 150 | * \brief Display resulting path from a solver, in the form of a planner_data 151 | * where the list of states is also the order of the path. This uses MoveIt's robot state for inverse 152 | * kinematics 153 | * \return true on success 154 | */ 155 | RVIZ_VISUAL_TOOLS_DEPRECATED 156 | bool publishRobotPath(const ompl::base::PlannerDataPtr& path, robot_model::JointModelGroup* jmg, 157 | const std::vector& tips, bool show_trajectory_animated) 158 | { 159 | return publishTrajectoryPath(path, jmg, tips, show_trajectory_animated); 160 | } 161 | 162 | RVIZ_VISUAL_TOOLS_DEPRECATED 163 | bool publishRobotPath(const og::PathGeometric& path, const robot_model::JointModelGroup* jmg, const bool blocking) 164 | { 165 | return publishTrajectoryPath(path, jmg, blocking); 166 | } 167 | 168 | bool publishTrajectoryPath(const ompl::base::PlannerDataPtr& path, robot_model::JointModelGroup* jmg, 169 | const std::vector& tips, bool show_trajectory_animated); 170 | 171 | bool publishTrajectoryPath(const og::PathGeometric& path, const robot_model::JointModelGroup* jmg, 172 | const bool blocking); 173 | 174 | /** 175 | * \brief Display result path from a solver 176 | * \return true on success 177 | */ 178 | RVIZ_VISUAL_TOOLS_DEPRECATED 179 | bool publishPath(const og::PathGeometric& path, const rviz_visual_tools::colors& color, const double thickness = 0.4, 180 | const std::string& ns = "result_path"); 181 | bool publish2DPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness = 0.4, 182 | const std::string& ns = "result_path"); 183 | 184 | /** 185 | * \brief Helper Function: gets the x,y coordinates for a given vertex id 186 | * \param id of a vertex 187 | * \param result from an OMPL planner 188 | * \return geometry point msg with no z value filled in 189 | */ 190 | Eigen::Vector3d stateToPoint(const ob::ScopedState<> state); 191 | Eigen::Vector3d stateToPoint(const ob::State* state); 192 | 193 | /** 194 | * \brief Display the start and goal states on the image map 195 | * \param start state 196 | * \param color 197 | */ 198 | bool publishState(const ob::State* state, const rviz_visual_tools::colors& color, 199 | const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR, 200 | const std::string& ns = "state_sphere"); 201 | bool publishState(const ob::State* state, const rviz_visual_tools::colors& color, const double scale = 0.1, 202 | const std::string& ns = "state_sphere"); 203 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, 204 | const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR, 205 | const std::string& ns = "state_sphere"); 206 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, double scale = 0.1, 207 | const std::string& ns = "state_sphere"); 208 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, 209 | const geometry_msgs::Vector3& scale, const std::string& ns = "state_sphere"); 210 | 211 | /** 212 | * \brief Visualize the sampling area in Rviz 213 | * \param state_area - the center point of the uniform sampler 214 | * \param distance - the radius around the center for sampling 215 | */ 216 | bool publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance); 217 | 218 | /** 219 | * \brief Publish text to rviz at a given location 220 | */ 221 | // bool publishText(const geometry_msgs::Point& point, const std::string& text, 222 | // const rviz_visual_tools::colors& color = rviz_visual_tools::BLACK, bool static_id = true); 223 | 224 | // bool publishText(const geometry_msgs::Pose& pose, const std::string& text, 225 | // const rviz_visual_tools::colors& color = rviz_visual_tools::BLACK, bool static_id = true); 226 | 227 | /** 228 | * \brief Convet each vertex in a graph into a list of tip locations, as desired 229 | * \param input - description 230 | * \param input - description 231 | * \return 232 | */ 233 | // bool convertRobotStatesToTipPoints(const ompl::base::PlannerDataPtr& graph, 234 | // const std::vector& tips, 235 | // std::vector >& vertex_tip_points); 236 | 237 | /** \brief Convert path formats */ 238 | bool convertPath(const og::PathGeometric& path, const robot_model::JointModelGroup* jmg, 239 | robot_trajectory::RobotTrajectoryPtr& traj, double speed = 0.1); 240 | 241 | /** 242 | * \brief Set the range to visualize the edge costs 243 | * \param invert - if true, red is largest values and green is lowest 244 | */ 245 | void setMinMaxEdgeCost(const double& min_edge_cost, const double& max_edge_cost, bool invert = false) 246 | { 247 | min_edge_cost_ = min_edge_cost; 248 | max_edge_cost_ = max_edge_cost; 249 | invert_edge_cost_ = invert; 250 | } 251 | 252 | void setMinMaxEdgeRadius(const double& min_edge_radius, const double& max_edge_radius) 253 | { 254 | min_edge_radius_ = min_edge_radius; 255 | max_edge_radius_ = max_edge_radius; 256 | } 257 | 258 | void setMinMaxStateRadius(const double& min_state_radius, const double& max_state_radius) 259 | { 260 | min_state_radius_ = min_state_radius; 261 | max_state_radius_ = max_state_radius; 262 | } 263 | 264 | /** 265 | * \brief An OMPL planner calls this function directly through boost::bind to display its graph's progress during 266 | * search 267 | * \param pointer to the planner, to be used for getPlannerData() 268 | */ 269 | void vizTrigger(); 270 | 271 | /** \brief Getter for JointModelGroup */ 272 | const robot_model::JointModelGroup* getJointModelGroup() const 273 | { 274 | return jmg_; 275 | } 276 | 277 | /** \brief Setter for JointModelGroup */ 278 | void setJointModelGroup(const robot_model::JointModelGroup* jmg) 279 | { 280 | jmg_ = jmg; 281 | } 282 | 283 | /** \brief Getter for SpaceInformation */ 284 | const ompl::base::SpaceInformationPtr& getSpaceInformation() const 285 | { 286 | return si_; 287 | } 288 | 289 | private: 290 | /** \brief Short name of class */ 291 | std::string name_; 292 | 293 | /** \brief Rviz visualization tools */ 294 | moveit_visual_tools::MoveItVisualToolsPtr visuals_; 295 | 296 | /** \brief Remember what space we are working in */ 297 | ompl::base::SpaceInformationPtr si_; 298 | 299 | // From ompl_visual_tools ------------------------------------------------------ 300 | 301 | // Remember what joint model group we care about so that calls from OMPL don't have to 302 | const robot_model::JointModelGroup* jmg_; 303 | 304 | // Cached Point object to reduce memory loading 305 | geometry_msgs::Point temp_point_; 306 | Eigen::Vector3d temp_eigen_point_; 307 | 308 | // Set bounds on an edge's cost/weight/value for visualization purposes 309 | double max_edge_cost_ = 100.0; 310 | double min_edge_cost_ = 0.0; 311 | bool invert_edge_cost_ = false; 312 | double max_edge_radius_ = 0.5; 313 | double min_edge_radius_ = 0.1; 314 | 315 | // Set bounds on an state's cost/weight/value for visualization purposes 316 | double max_state_radius_ = 0.1; 317 | double min_state_radius_ = 0.5; 318 | 319 | double level_scale_ = 20.0; 320 | }; 321 | 322 | typedef std::shared_ptr MoveItVizWindowPtr; 323 | typedef std::shared_ptr MoveItVizWindowConstPtr; 324 | 325 | } // namespace ompl_visual_tools 326 | 327 | #endif // OMPL_VISUAL_TOOLS__MOVEIT_VIZ_WINDOW_ 328 | -------------------------------------------------------------------------------- /include/ompl_visual_tools/projection_viz_window.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tools for displaying OMPL components in Rviz 37 | */ 38 | 39 | #ifndef OMPL_VISUAL_TOOLS__PROJECTION_VIZ_WINDOW_ 40 | #define OMPL_VISUAL_TOOLS__PROJECTION_VIZ_WINDOW_ 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | // OMPL 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | // Custom validity checker that accounts for cost 55 | #include 56 | 57 | // Visualization 58 | #include 59 | 60 | namespace ob = ompl::base; 61 | namespace og = ompl::geometric; 62 | namespace bnu = boost::numeric::ublas; 63 | namespace rvt = rviz_visual_tools; 64 | 65 | namespace ompl_interface 66 | { 67 | class ModelBasedPlanningContext; 68 | typedef std::shared_ptr ModelBasedPlanningContextPtr; 69 | } 70 | 71 | namespace ompl_visual_tools 72 | { 73 | // Default constants 74 | // static const std::string RVIZ_MARKER_TOPIC = "/ompl_rviz_markers"; 75 | // static const double COST_HEIGHT_OFFSET = 0.5; 76 | 77 | typedef std::map > MarkerList; 78 | 79 | class ProjectionVizWindow : public ompl::tools::VizWindow 80 | { 81 | public: 82 | ProjectionVizWindow(rviz_visual_tools::RvizVisualToolsPtr visuals, ompl::base::SpaceInformationPtr si); 83 | 84 | /** \brief Visualize a state during runtime, externally */ 85 | void state(const ompl::base::State* state, ompl::tools::VizSizes size, ompl::tools::VizColors color, 86 | double extraData); 87 | 88 | /** \brief Visualize multiple states during runtime, externally */ 89 | void states(std::vector states, std::vector colors, 90 | ompl::tools::VizSizes size); 91 | 92 | /** \brief Visualize edge during runtime, externally */ 93 | void edge(const ompl::base::State* stateA, const ompl::base::State* stateB, double cost); 94 | 95 | /** \brief Visualize edge with a level during runtime, externally */ 96 | void edge(const ompl::base::State* stateA, const ompl::base::State* stateB, ompl::tools::VizSizes size, 97 | ompl::tools::VizColors color); 98 | 99 | /** \brief Visualize multiple edges during runtime, externally */ 100 | void edges(const std::vector stateAs, const std::vector stateBs, 101 | std::vector colors, ompl::tools::VizSizes size); 102 | 103 | /** \brief Visualize path during runtime, externally */ 104 | void path(ompl::geometric::PathGeometric* path, ompl::tools::VizSizes type, ompl::tools::VizColors color); 105 | 106 | /** \brief Trigger visualizer to refresh/repaint/display all graphics */ 107 | void trigger(); 108 | 109 | /** \brief Trigger visualizer to clear all graphics */ 110 | void deleteAllMarkers(); 111 | 112 | /** \brief Check if SIGINT has been called */ 113 | bool shutdownRequested(); 114 | 115 | /** \brief Get the underlying visualizer */ 116 | rviz_visual_tools::RvizVisualToolsPtr getVisualTools() 117 | { 118 | return visuals_; 119 | } 120 | 121 | // From ompl_visual_tools ------------------------------------------------------ 122 | 123 | /** 124 | * \brief Publish a marker of a series of spheres to rviz 125 | * \param spheres - where to publish them 126 | * \param color - an enum pre-defined name of a color 127 | * \param scale - an enum pre-defined name of a size 128 | * \param ns - namespace of marker 129 | * \return true on success 130 | */ 131 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color = rviz_visual_tools::RED, 132 | double scale = 0.1, const std::string& ns = "path_spheres"); 133 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color = rviz_visual_tools::RED, 134 | const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, 135 | const std::string& ns = "path_spheres"); 136 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color, 137 | const geometry_msgs::Vector3& scale, const std::string& ns = "path_spheres"); 138 | 139 | /** 140 | * \brief Display a connection between two states as a straight line 141 | */ 142 | bool publishEdge(const ob::State* stateA, const ob::State* stateB, const std_msgs::ColorRGBA& color, 143 | const double radius = 0.05); 144 | 145 | /** 146 | * \brief Display result path from a solver 147 | * \return true on success 148 | */ 149 | bool publishPath(const og::PathGeometric& path, const rviz_visual_tools::colors& color, const double thickness = 0.4, 150 | const std::string& ns = "result_path"); 151 | 152 | bool publish2DPath(const og::PathGeometric& path, const rviz_visual_tools::colors& color, 153 | const double thickness = 0.4, const std::string& ns = "result_path"); 154 | 155 | /** 156 | * \brief Helper Function: gets the x,y coordinates for a given vertex id 157 | * \param id of a vertex 158 | * \param result from an OMPL planner 159 | * \return geometry point msg with no z value filled in 160 | */ 161 | // Eigen::Vector3d stateToPoint(std::size_t vertex_id, ob::PlannerDataPtr planner_data); 162 | Eigen::Vector3d stateToPoint(const ob::ScopedState<> state); 163 | Eigen::Vector3d stateToPoint(const ob::State* state); 164 | 165 | /** 166 | * \brief Nat_Rounding helper function to make readings from cost map more accurate 167 | * \param double 168 | * \return rounded down number 169 | */ 170 | static int natRound(double x); 171 | 172 | /** 173 | * \brief Display the start and goal states on the image map 174 | * \param start state 175 | * \param color 176 | */ 177 | bool publishState(const ob::State* state, const rviz_visual_tools::colors& color, 178 | const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR, 179 | const std::string& ns = "state_sphere"); 180 | bool publishState(const ob::State* state, const rviz_visual_tools::colors& color, const double scale = 0.1, 181 | const std::string& ns = "state_sphere"); 182 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, 183 | const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR, 184 | const std::string& ns = "state_sphere"); 185 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, double scale = 0.1, 186 | const std::string& ns = "state_sphere"); 187 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, 188 | const geometry_msgs::Vector3& scale, const std::string& ns = "state_sphere"); 189 | 190 | /** 191 | * \brief Visualize the sampling area in Rviz 192 | * \param state_area - the center point of the uniform sampler 193 | * \param distance - the radius around the center for sampling 194 | */ 195 | bool publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance); 196 | 197 | /** 198 | * \brief Set the range to visualize the edge costs 199 | * \param invert - if true, red is largest values and green is lowest 200 | */ 201 | void setMinMaxEdgeCost(const double& min_edge_cost, const double& max_edge_cost, bool invert = false) 202 | { 203 | min_edge_cost_ = min_edge_cost; 204 | max_edge_cost_ = max_edge_cost; 205 | invert_edge_cost_ = invert; 206 | } 207 | 208 | void setMinMaxEdgeRadius(const double& min_edge_radius, const double& max_edge_radius) 209 | { 210 | min_edge_radius_ = min_edge_radius; 211 | max_edge_radius_ = max_edge_radius; 212 | } 213 | 214 | void setMinMaxStateRadius(const double& min_state_radius, const double& max_state_radius) 215 | { 216 | min_state_radius_ = min_state_radius; 217 | max_state_radius_ = max_state_radius; 218 | } 219 | 220 | /** \brief Getter for SpaceInformation */ 221 | const ompl::base::SpaceInformationPtr& getSpaceInformation() const 222 | { 223 | return si_; 224 | } 225 | 226 | private: 227 | /** \brief Short name of class */ 228 | std::string name_; 229 | 230 | /** \brief Rviz visualization tools */ 231 | rviz_visual_tools::RvizVisualToolsPtr visuals_; 232 | 233 | /** \brief Remember what space we are working in */ 234 | ompl::base::SpaceInformationPtr si_; 235 | 236 | // From ompl_visual_tools ------------------------------------------------------ 237 | 238 | // Keep a pointer to an optional cost map 239 | intMatrixPtr cost_; 240 | 241 | // Cached Point object to reduce memory loading 242 | geometry_msgs::Point temp_point_; 243 | Eigen::Vector3d temp_eigen_point_; 244 | 245 | // Set bounds on an edge's cost/weight/value for visualization purposes 246 | double max_edge_cost_ = 100.0; 247 | double min_edge_cost_ = 0.0; 248 | bool invert_edge_cost_ = false; 249 | double max_edge_radius_ = 0.5; 250 | double min_edge_radius_ = 0.1; 251 | 252 | // Set bounds on an state's cost/weight/value for visualization purposes 253 | double max_state_radius_ = 0.1; 254 | double min_state_radius_ = 0.5; 255 | 256 | double level_scale_ = 20.0; 257 | 258 | // Properties of state space 259 | std::vector range_; 260 | std::vector low_; 261 | }; 262 | 263 | typedef std::shared_ptr ProjectionVizWindowPtr; 264 | typedef std::shared_ptr ProjectionVizWindowConstPtr; 265 | 266 | } // namespace ompl_visual_tools 267 | 268 | #endif // OMPL_VISUAL_TOOLS__PROJECTION_VIZ_WINDOW_ 269 | -------------------------------------------------------------------------------- /include/ompl_visual_tools/ros_viz_window.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tools for displaying OMPL components in Rviz 37 | */ 38 | 39 | #ifndef OMPL_VISUAL_TOOLS__ROS_VIZ_WINDOW_ 40 | #define OMPL_VISUAL_TOOLS__ROS_VIZ_WINDOW_ 41 | 42 | #include 43 | //#include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | // OMPL 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | // Custom validity checker that accounts for cost 56 | #include 57 | 58 | // Visualization 59 | #include 60 | 61 | namespace ob = ompl::base; 62 | namespace og = ompl::geometric; 63 | namespace bnu = boost::numeric::ublas; 64 | namespace rvt = rviz_visual_tools; 65 | 66 | namespace ompl_visual_tools 67 | { 68 | class ROSVizWindow : public ompl::tools::VizWindow 69 | { 70 | public: 71 | ROSVizWindow(rviz_visual_tools::RvizVisualToolsPtr visuals, ompl::base::SpaceInformationPtr si); 72 | 73 | /** \brief Visualize a state during runtime, externally */ 74 | void state(const ompl::base::State* state, ompl::tools::VizSizes size, ompl::tools::VizColors color, 75 | double extraData); 76 | 77 | /** \brief Visualize multiple states during runtime, externally */ 78 | void states(std::vector states, std::vector colors, ompl::tools::VizSizes size); 79 | 80 | /** \brief Visualize edge with a level during runtime, externally */ 81 | void edge(const ompl::base::State* stateA, const ompl::base::State* stateB, ompl::tools::VizSizes size, 82 | ompl::tools::VizColors color); 83 | 84 | /** \brief Visualize edge during runtime, externally */ 85 | void edge(const ompl::base::State* stateA, const ompl::base::State* stateB, double cost); 86 | 87 | /** \brief Visualize multiple edges during runtime, externally */ 88 | // void edges(const std::vector stateAs, const std::vector stateBs, 89 | // std::vector colors, ompl::tools::VizSizes size){}; 90 | 91 | /** \brief Visualize path during runtime, externally */ 92 | void path(ompl::geometric::PathGeometric* path, ompl::tools::VizSizes type, ompl::tools::VizColors color); 93 | 94 | /** \brief Trigger visualizer to refresh/repaint/display all graphics */ 95 | void trigger(); 96 | 97 | /** \brief Trigger visualizer to clear all graphics */ 98 | void deleteAllMarkers(); 99 | 100 | /** \brief Check if SIGINT has been called */ 101 | bool shutdownRequested(); 102 | 103 | /** \brief Get the underlying visualizer */ 104 | rviz_visual_tools::RvizVisualToolsPtr getVisualTools() 105 | { 106 | return visuals_; 107 | } 108 | 109 | // From ompl_visual_tools ------------------------------------------------------ 110 | 111 | /** 112 | * \brief Optional cost map for 2D environments 113 | */ 114 | void setCostMap(intMatrixPtr cost); 115 | 116 | /** 117 | * \brief Helper function for converting a point to the correct cost 118 | */ 119 | double getCost(const geometry_msgs::Point& point); 120 | 121 | /** 122 | * \brief Visualize Results 123 | */ 124 | bool publishCostMap(PPMImage* image, bool static_id = true); 125 | 126 | /** 127 | * \brief Helper Function to display triangles 128 | */ 129 | bool publishTriangle(int x, int y, visualization_msgs::Marker* marker, std_msgs::ColorRGBA color); 130 | 131 | /** 132 | * \brief Publish a marker of a series of spheres to rviz 133 | * \param spheres - where to publish them 134 | * \param color - an enum pre-defined name of a color 135 | * \param scale - an enum pre-defined name of a size 136 | * \param ns - namespace of marker 137 | * \return true on success 138 | */ 139 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color = rviz_visual_tools::RED, 140 | double scale = 0.1, const std::string& ns = "path_spheres"); 141 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color = rviz_visual_tools::RED, 142 | const rviz_visual_tools::scales scale = rviz_visual_tools::SMALL, 143 | const std::string& ns = "path_spheres"); 144 | bool publishSpheres(const og::PathGeometric& path, const rviz_visual_tools::colors& color, 145 | const geometry_msgs::Vector3& scale, const std::string& ns = "path_spheres"); 146 | 147 | /** 148 | * \brief Display a connection between two states as a straight line 149 | */ 150 | bool publishEdge(const ob::State* stateA, const ob::State* stateB, const std_msgs::ColorRGBA& color, 151 | const double radius = 0.05); 152 | 153 | /** 154 | * \brief Display States 155 | * \return true on success 156 | */ 157 | //bool publishStates(std::vector states); 158 | 159 | /** 160 | * \brief Display result path from a solver 161 | * \return true on success 162 | */ 163 | bool publishPath(const og::PathGeometric& path, const rviz_visual_tools::colors& color, const double thickness = 0.4, 164 | const std::string& ns = "result_path"); 165 | 166 | bool publish2DPath(const og::PathGeometric& path, const rviz_visual_tools::colors& color, 167 | const double thickness = 0.4, const std::string& ns = "result_path"); 168 | 169 | /** 170 | * \brief Helper Function: gets the x,y coordinates for a given vertex id 171 | * \param id of a vertex 172 | * \param result from an OMPL planner 173 | * \return geometry point msg with no z value filled in 174 | */ 175 | // Eigen::Vector3d stateToPoint(std::size_t vertex_id, ob::PlannerDataPtr planner_data); 176 | Eigen::Vector3d stateToPoint(const ob::ScopedState<> state); 177 | Eigen::Vector3d stateToPoint(const ob::State* state); 178 | 179 | /** 180 | * \brief Nat_Rounding helper function to make readings from cost map more accurate 181 | * \param double 182 | * \return rounded down number 183 | */ 184 | static int natRound(double x); 185 | 186 | /** 187 | * \brief Display the start and goal states on the image map 188 | * \param start state 189 | * \param color 190 | */ 191 | bool publishState(const ob::State* state, const rviz_visual_tools::colors& color, 192 | const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR, 193 | const std::string& ns = "state_sphere"); 194 | bool publishState(const ob::State* state, const rviz_visual_tools::colors& color, const double scale = 0.1, 195 | const std::string& ns = "state_sphere"); 196 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, 197 | const rviz_visual_tools::scales scale = rviz_visual_tools::REGULAR, 198 | const std::string& ns = "state_sphere"); 199 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, double scale = 0.1, 200 | const std::string& ns = "state_sphere"); 201 | bool publishState(const ob::ScopedState<> state, const rviz_visual_tools::colors& color, 202 | const geometry_msgs::Vector3& scale, const std::string& ns = "state_sphere"); 203 | 204 | /** 205 | * \brief Visualize the sampling area in Rviz 206 | * \param state_area - the center point of the uniform sampler 207 | * \param distance - the radius around the center for sampling 208 | */ 209 | bool publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance); 210 | 211 | /** 212 | * \brief Set the range to visualize the edge costs 213 | * \param invert - if true, red is largest values and green is lowest 214 | */ 215 | void setMinMaxEdgeCost(const double& min_edge_cost, const double& max_edge_cost, bool invert = false) 216 | { 217 | min_edge_cost_ = min_edge_cost; 218 | max_edge_cost_ = max_edge_cost; 219 | invert_edge_cost_ = invert; 220 | } 221 | 222 | void setMinMaxEdgeRadius(const double& min_edge_radius, const double& max_edge_radius) 223 | { 224 | min_edge_radius_ = min_edge_radius; 225 | max_edge_radius_ = max_edge_radius; 226 | } 227 | 228 | void setMinMaxStateRadius(const double& min_state_radius, const double& max_state_radius) 229 | { 230 | min_state_radius_ = min_state_radius; 231 | max_state_radius_ = max_state_radius; 232 | } 233 | 234 | /** \brief Getter for SpaceInformation */ 235 | const ompl::base::SpaceInformationPtr& getSpaceInformation() const 236 | { 237 | return si_; 238 | } 239 | 240 | private: 241 | /** \brief Short name of class */ 242 | std::string name_; 243 | 244 | /** \brief Rviz visualization tools */ 245 | rviz_visual_tools::RvizVisualToolsPtr visuals_; 246 | 247 | /** \brief Remember what space we are working in */ 248 | ompl::base::SpaceInformationPtr si_; 249 | 250 | // From ompl_visual_tools ------------------------------------------------------ 251 | 252 | // Keep a pointer to an optional cost map 253 | intMatrixPtr cost_; 254 | 255 | // Cached Point object to reduce memory loading 256 | geometry_msgs::Point temp_point_; 257 | Eigen::Vector3d temp_eigen_point_; 258 | 259 | // Set bounds on an edge's cost/weight/value for visualization purposes 260 | double max_edge_cost_ = 100.0; 261 | double min_edge_cost_ = 0.0; 262 | bool invert_edge_cost_ = false; 263 | double max_edge_radius_ = 0.5; 264 | double min_edge_radius_ = 0.1; 265 | 266 | // Set bounds on an state's cost/weight/value for visualization purposes 267 | double max_state_radius_ = 0.1; 268 | double min_state_radius_ = 0.5; 269 | 270 | double level_scale_ = 20.0; 271 | }; 272 | 273 | typedef std::shared_ptr ROSVizWindowPtr; 274 | typedef std::shared_ptr ROSVizWindowConstPtr; 275 | 276 | } // namespace ompl_visual_tools 277 | 278 | #endif // OMPL_VISUAL_TOOLS__ROS_VIZ_WINDOW_ 279 | -------------------------------------------------------------------------------- /include/ompl_visual_tools/utilities/ppm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PPM Image Format Reader 3 | * Code adapted from http://stackoverflow.com/questions/2693631/read-ppm-file-and-store-it-in-an-array-coded-with-c 4 | * 8/1/2012 5 | * License Unkown 6 | */ 7 | 8 | #ifndef OMPL_VISUAL_TOOLS_PPM_ 9 | #define OMPL_VISUAL_TOOLS_PPM_ 10 | 11 | #include 12 | #include 13 | 14 | namespace ompl_visual_tools 15 | { 16 | 17 | // ************************************************************************************************* 18 | // PPMPixel Struct 19 | // ************************************************************************************************* 20 | typedef struct { 21 | unsigned char red,green,blue; 22 | } PPMPixel; 23 | 24 | // ************************************************************************************************* 25 | // PPMImage Struct 26 | // ************************************************************************************************* 27 | class PPMImage 28 | { 29 | public: 30 | // Constructor 31 | PPMImage() : 32 | x(0), y(0), data(NULL) 33 | { 34 | } 35 | 36 | // Deconstructor 37 | ~PPMImage() 38 | { 39 | if( data != NULL ) 40 | free(data); 41 | } 42 | 43 | // Convert coordinates to a id number 44 | unsigned int getID( unsigned int x_coord, unsigned int y_coord ) 45 | { 46 | return y_coord * x + x_coord; 47 | } 48 | 49 | // Convert id to x 50 | unsigned int getX( unsigned int id ) 51 | { 52 | return id % x; 53 | } 54 | 55 | // Convert id to y 56 | unsigned int getY( unsigned int id ) 57 | { 58 | return id / x; 59 | } 60 | 61 | unsigned int getSize() 62 | { 63 | return x * y; 64 | } 65 | 66 | // Member variables 67 | unsigned int x, y; 68 | PPMPixel *data; 69 | }; 70 | 71 | 72 | #define CREATOR "RPFELGUEIRAS" 73 | #define RGB_COMPONENT_COLOR 255 74 | 75 | // ************************************************************************************************* 76 | // Read Function 77 | // ************************************************************************************************* 78 | 79 | PPMImage *readPPM(const char *filename); 80 | 81 | 82 | } // namespace 83 | 84 | #endif 85 | 86 | 87 | -------------------------------------------------------------------------------- /launch/ompl_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/ompl_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /OMPL Visual Tools1 9 | - /OMPL Visual Tools1/Namespaces1 10 | Splitter Ratio: 0.784387 11 | Tree Height: 1286 12 | - Class: rviz/Help 13 | Name: Help 14 | - Class: rviz/Views 15 | Expanded: 16 | - /Current View1 17 | Name: Views 18 | Splitter Ratio: 0.5 19 | Visualization Manager: 20 | Class: "" 21 | Displays: 22 | - Class: rviz/Marker 23 | Enabled: true 24 | Marker Topic: /ompl_rviz_markers 25 | Name: OMPL Visual Tools 26 | Namespaces: 27 | cost_map: false 28 | final_solution: true 29 | plan_from_scratch: true 30 | plan_start_goal: true 31 | text: true 32 | Queue Size: 10000 33 | Value: true 34 | Enabled: true 35 | Global Options: 36 | Background Color: 255; 255; 255 37 | Fixed Frame: world 38 | Frame Rate: 30 39 | Name: root 40 | Tools: 41 | - Class: rviz/Interact 42 | Hide Inactive Objects: true 43 | - Class: rviz/MoveCamera 44 | Value: true 45 | Views: 46 | Current: 47 | Class: rviz/Orbit 48 | Distance: 1030.42 49 | Enable Stereo Rendering: 50 | Stereo Eye Separation: 0.06 51 | Stereo Focal Distance: 1 52 | Swap Stereo Eyes: false 53 | Value: false 54 | Focal Point: 55 | X: 209.965 56 | Y: 150.492 57 | Z: -37.2218 58 | Name: Current View 59 | Near Clip Distance: 0.01 60 | Pitch: 1.4398 61 | Target Frame: 62 | Value: Orbit (rviz) 63 | Yaw: 1.57027 64 | Saved: ~ 65 | Window Geometry: 66 | Displays: 67 | collapsed: false 68 | Height: 1524 69 | Help: 70 | collapsed: false 71 | Hide Left Dock: false 72 | Hide Right Dock: false 73 | QMainWindow State: 000000ff00000000fd00000001000000000000010f00000595fc0200000003fb000000100044006900730070006c006100790073010000004100000595000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a00560069006500770073000000031d000000dd000000b000ffffff000003eb0000059500000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 74 | Views: 75 | collapsed: false 76 | Width: 1280 77 | X: -3 78 | Y: 24 79 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | ompl_visual_tools 3 | 2.3.2 4 | 5 | Rviz 3-D visualizer for planning algorithms implemented with the Open Motion Planning Library (OMPL) 6 | 7 | 8 | Dave Coleman 9 | Dave Coleman 10 | 11 | BSD 12 | 13 | http://ros.org/wiki/ompl_visual_tools 14 | https://github.com/davetcoleman/ompl_visual_tools/issues 15 | https://github.com/davetcoleman/ompl_visual_tools/ 16 | 17 | catkin 18 | 19 | graph_msgs 20 | moveit_visual_tools 21 | moveit_ompl 22 | moveit_core 23 | roscpp 24 | roslib 25 | visualization_msgs 26 | ompl 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /plot/plot.pg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/gnuplot 2 | reset 3 | #set terminal png 4 | 5 | set terminal wxt size 1800,1000 6 | 7 | set xlabel "Samples" 8 | 9 | set ylabel "Temp" 10 | 11 | set yrange [0:0.05] 12 | #set autoscale 13 | 14 | set title "OMPL T-RRT" 15 | set key reverse Left inside 16 | set grid 17 | 18 | set style data lines 19 | #set style line 1 lw 6 20 | 21 | 22 | set datafile separator "," 23 | 24 | plot "temperatures.dat" using 1:2 title 'Temperature' linecolor rgb "green" 25 | # "temperatures.dat" using 1:3 title 'Ratio' linecolor rgb "red" 26 | 27 | 28 | #pause 10 29 | #reread -------------------------------------------------------------------------------- /plot/run_plot.sh: -------------------------------------------------------------------------------- 1 | gnuplot -persist plot.pg 2 | -------------------------------------------------------------------------------- /resources/grand_canyon.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/grand_canyon.gif -------------------------------------------------------------------------------- /resources/grand_canyon.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/grand_canyon.ppm -------------------------------------------------------------------------------- /resources/height_map0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/height_map0.png -------------------------------------------------------------------------------- /resources/height_map0.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/height_map0.ppm -------------------------------------------------------------------------------- /resources/height_map1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/height_map1.jpg -------------------------------------------------------------------------------- /resources/height_map1.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/height_map1.ppm -------------------------------------------------------------------------------- /resources/height_map2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/height_map2.jpg -------------------------------------------------------------------------------- /resources/height_map2.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/height_map2.ppm -------------------------------------------------------------------------------- /resources/wilbur_medium/wilbur_medium1.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/wilbur_medium/wilbur_medium1.ppm -------------------------------------------------------------------------------- /resources/wilbur_medium/wilbur_medium2.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/resources/wilbur_medium/wilbur_medium2.ppm -------------------------------------------------------------------------------- /screenshots/ompl_visual_tools.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/screenshots/ompl_visual_tools.png -------------------------------------------------------------------------------- /screenshots/repaired_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/screenshots/repaired_path.png -------------------------------------------------------------------------------- /screenshots/similar_paths.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/ompl_visual_tools/4882529b9c46b4e6959c98e7344b7179d0a1857e/screenshots/similar_paths.png -------------------------------------------------------------------------------- /src/demos/rrt_demo.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * Copyright (c) 2012, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /* 37 | Author: Dave Coleman 38 | Desc: Visualize planning with OMPL in Rviz 39 | */ 40 | 41 | // ROS 42 | #include 43 | #include // for getting file path for loading images 44 | 45 | // Display in Rviz tool 46 | #include 47 | #include 48 | 49 | // OMPL 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | // Boost 59 | #include 60 | 61 | namespace ob = ompl::base; 62 | namespace og = ompl::geometric; 63 | 64 | namespace ompl_visual_tools 65 | { 66 | 67 | static const std::string BASE_FRAME = "/world"; 68 | 69 | /** 70 | * \brief SimpleSetup demo class 71 | */ 72 | class RRTDemo 73 | { 74 | 75 | private: 76 | 77 | og::SimpleSetupPtr simple_setup_; 78 | 79 | // Cost in 2D 80 | ompl::base::CostMap2DOptimizationObjectivePtr cost_map_; 81 | 82 | // Cost is just path length 83 | std::shared_ptr path_length_objective_; 84 | 85 | // The visual tools for interfacing with Rviz 86 | ompl_visual_tools::OmplVisualToolsPtr visual_tools_; 87 | 88 | // The number of dimensions - always 2 for images 89 | static const unsigned int DIMENSIONS = 2; 90 | 91 | // The state space we plan in 92 | ob::StateSpacePtr space_; 93 | 94 | // Remember what space we are working in 95 | ompl::base::SpaceInformationPtr si_; 96 | 97 | // Flag for determining amount of debug output to show 98 | bool verbose_; 99 | 100 | // Display graphics in Rviz 101 | bool use_visuals_; 102 | 103 | public: 104 | 105 | /** 106 | * \brief Constructor 107 | */ 108 | RRTDemo(bool verbose, bool use_visuals) 109 | : verbose_(verbose), 110 | use_visuals_(use_visuals) 111 | { 112 | // Construct the state space we are planning in 113 | space_.reset( new ob::RealVectorStateSpace( DIMENSIONS )); 114 | 115 | // Define an experience setup class 116 | simple_setup_ = og::SimpleSetupPtr( new og::SimpleSetup(space_) ); 117 | si_ = simple_setup_->getSpaceInformation(); 118 | 119 | // Load the tool for displaying in Rviz 120 | visual_tools_.reset(new ompl_visual_tools::OmplVisualTools(BASE_FRAME)); 121 | visual_tools_->setSpaceInformation(si_); 122 | visual_tools_->setGlobalScale(100); 123 | 124 | // Set the planner 125 | //simple_setup_->setPlanner(ob::PlannerPtr(new og::RRTstar( si_ ))); 126 | simple_setup_->setPlanner(ob::PlannerPtr(new og::RRT( si_ ))); 127 | 128 | // Load the cost map 129 | cost_map_.reset(new ompl::base::CostMap2DOptimizationObjective( si_ )); 130 | 131 | // Load an alternitive optimization objective 132 | path_length_objective_.reset(new ompl::base::PathLengthOptimizationObjective( si_ )); 133 | } 134 | 135 | /** 136 | * \brief Deconstructor 137 | */ 138 | ~RRTDemo() 139 | { 140 | } 141 | 142 | /** 143 | * \brief Clear all markers displayed in Rviz 144 | */ 145 | void deleteAllMakers() 146 | { 147 | // Clear current rviz makers 148 | visual_tools_->deleteAllMarkers(); 149 | } 150 | 151 | /** 152 | * \brief Load cost map from file 153 | * \param file path 154 | * \param how much of the peaks of the mountains are considered obstacles 155 | */ 156 | void loadCostMapImage( std::string image_path, double max_cost_threshold_percent = 0.4 ) 157 | { 158 | cost_map_->max_cost_threshold_percent_ = max_cost_threshold_percent; 159 | cost_map_->loadImage(image_path); 160 | 161 | // Set the bounds for the R^2 162 | ob::RealVectorBounds bounds( DIMENSIONS ); 163 | bounds.setLow( 0 ); // both dimensions start at 0 164 | bounds.setHigh( 0, cost_map_->image_->x - 1 ); // allow for non-square images 165 | bounds.setHigh( 1, cost_map_->image_->y - 1 ); // allow for non-square images 166 | space_->as()->setBounds( bounds ); 167 | space_->setup(); 168 | 169 | // Pass cost to visualizer 170 | visual_tools_->setCostMap(cost_map_->cost_); 171 | 172 | // Set state validity checking for this space 173 | simple_setup_->setStateValidityChecker( ob::StateValidityCheckerPtr( 174 | new ob::TwoDimensionalValidityChecker( si_, cost_map_->cost_, cost_map_->max_cost_threshold_ ) ) ); 175 | 176 | // The interval in which obstacles are checked for between states 177 | // seems that it default to 0.01 but doesn't do a good job at that level 178 | si_->setStateValidityCheckingResolution(0.001); 179 | 180 | // Setup the optimization objective to use the 2d cost map 181 | // NEWER VERSION OF OMPL 182 | //simple_setup_->setOptimizationObjective(cost_map_); 183 | //simple_setup_->setOptimizationObjective(path_length_objective_); 184 | 185 | // Setup ----------------------------------------------------------- 186 | 187 | // Auto setup parameters 188 | simple_setup_->setup(); // optional 189 | 190 | //ROS_ERROR_STREAM_NAMED("temp","out of curiosity: coll check resolution: " 191 | // << si_->getStateValidityCheckingResolution()); 192 | 193 | // Debug - this call is optional, but we put it in to get more output information 194 | //simple_setup_->print(); 195 | } 196 | 197 | void publishCostMapImage() 198 | { 199 | if (use_visuals_) 200 | visual_tools_->publishCostMap(cost_map_->image_); 201 | } 202 | 203 | /** 204 | * \brief Solve a planning proble that we randomly make up 205 | * \param run_id - which run this is 206 | * \param runs - how many total runs we will do 207 | * \return true on success 208 | */ 209 | bool plan(const int& run_id, const int& runs) 210 | { 211 | // Start and Goal State --------------------------------------------- 212 | ob::PlannerStatus solved; 213 | 214 | // Clear all planning data. This only includes data generated by motion plan computation. 215 | // Planner settings, start & goal states are not affected. 216 | if (run_id) // skip first run 217 | simple_setup_->clear(); 218 | 219 | // Clear the previous solutions 220 | //simple_setup_->getProblemDefinition()->clearSolutionPaths(); 221 | 222 | // Create the termination condition 223 | double seconds = 2; 224 | ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition( seconds, 0.1 ); 225 | 226 | // Create start and goal space 227 | ob::ScopedState<> start(space_); 228 | ob::ScopedState<> goal(space_); 229 | chooseStartGoal(start, goal); 230 | 231 | // Show start and goal 232 | if (use_visuals_) 233 | { 234 | visual_tools_->publishState(start, rviz_visual_tools::GREEN, rviz_visual_tools::XLARGE, "plan_start_goal"); 235 | visual_tools_->publishState(goal, rviz_visual_tools::ORANGE, rviz_visual_tools::XLARGE, "plan_start_goal"); 236 | } 237 | 238 | // set the start and goal states 239 | simple_setup_->setStartAndGoalStates(start, goal); 240 | 241 | // Solve ----------------------------------------------------------- 242 | 243 | // attempt to solve the problem within x seconds of planning time 244 | solved = simple_setup_->solve( ptc ); 245 | 246 | geometry_msgs::Pose text_pose; 247 | text_pose.position.x = cost_map_->cost_->size1()/2.0; 248 | text_pose.position.y = cost_map_->cost_->size1()/-20.0; 249 | text_pose.position.z = cost_map_->cost_->size1()/10.0; 250 | 251 | if (solved) 252 | { 253 | if (!simple_setup_->haveExactSolutionPath()) 254 | { 255 | ROS_WARN_STREAM_NAMED("plan","APPROXIMATE solution found"); 256 | if (use_visuals_) 257 | visual_tools_->publishText(text_pose, "APPROXIMATE solution found"); 258 | } 259 | else 260 | { 261 | ROS_DEBUG_STREAM_NAMED("plan","Exact solution found"); 262 | if (use_visuals_) 263 | visual_tools_->publishText(text_pose, "Exact solution found"); 264 | 265 | } 266 | 267 | if (use_visuals_) 268 | { 269 | if (runs == 1) 270 | { 271 | // display all the aspects of the solution 272 | publishPlannerData(false); 273 | } 274 | else 275 | { 276 | // only display the paths 277 | publishPlannerData(false); 278 | } 279 | } 280 | } 281 | else 282 | { 283 | ROS_ERROR("No Solution Found"); 284 | if (use_visuals_) 285 | visual_tools_->publishText(text_pose, "No Solution Found"); 286 | } 287 | 288 | return solved; 289 | } 290 | 291 | void chooseStartGoal(ob::ScopedState<>& start, ob::ScopedState<>& goal) 292 | { 293 | if( true ) // choose completely random state 294 | { 295 | findValidState(start); 296 | findValidState(goal); 297 | } 298 | else if (false) // Manually set the start location 299 | { 300 | // Plan from scrach location 301 | start[0] = 5; start[1] = 5; 302 | goal[0] = 5; goal[1] = 45; 303 | // Recall location 304 | start[0] = 45; start[1] = 5; 305 | goal[0] = 45; goal[1] = 45; 306 | } 307 | else // Randomly sample around two states 308 | { 309 | ROS_INFO_STREAM_NAMED("temp","Sampling start and goal around two center points"); 310 | 311 | ob::ScopedState<> start_area(space_); 312 | start_area[0] = 100; 313 | start_area[1] = 80; 314 | 315 | ob::ScopedState<> goal_area(space_); 316 | goal_area[0] = 330; 317 | goal_area[1] = 350; 318 | 319 | // Check these hard coded values against varying image sizes 320 | if (!space_->satisfiesBounds(start_area.get()) || !space_->satisfiesBounds(goal_area.get())) 321 | { 322 | ROS_ERROR_STREAM_NAMED("chooseStartGoal:","State does not satisfy bounds"); 323 | exit(-1); 324 | return; 325 | } 326 | 327 | // Choose the distance to sample around 328 | double maxExtent = si_->getMaximumExtent(); 329 | double distance = maxExtent * 0.1; 330 | ROS_INFO_STREAM_NAMED("temp","Distance is " << distance << " from max extent " << maxExtent); 331 | 332 | // Publish the same points 333 | if (true) 334 | { 335 | findValidState(start.get(), start_area.get(), distance); 336 | findValidState(goal.get(), goal_area.get(), distance); 337 | } 338 | else 339 | { 340 | // start[0] = 277.33; start[1] = 168.491; 341 | // goal[0] = 843.296; goal[1] = 854.184; 342 | } 343 | 344 | // Show the sample regions 345 | if (use_visuals_ && false) 346 | { 347 | visual_tools_->publishSampleRegion(start_area, distance); 348 | visual_tools_->publishSampleRegion(goal_area, distance); 349 | } 350 | } 351 | 352 | // Print the start and goal 353 | //ROS_DEBUG_STREAM_NAMED("chooseStartGoal","Start: " << start); 354 | //ROS_DEBUG_STREAM_NAMED("chooseStartGoal","Goal: " << goal); 355 | } 356 | 357 | void findValidState(ob::ScopedState<>& state) 358 | { 359 | std::size_t rounds = 0; 360 | while (rounds < 100) 361 | { 362 | state.random(); 363 | 364 | // Check if the sampled points are valid 365 | if( si_->isValid(state.get()) ) 366 | { 367 | return; 368 | } 369 | ++rounds; 370 | } 371 | ROS_ERROR_STREAM_NAMED("findValidState","Unable to find valid start/goal state after " << rounds << " rounds"); 372 | } 373 | 374 | void findValidState(ob::State *state, const ob::State *near, const double distance) 375 | { 376 | // Create sampler 377 | ob::StateSamplerPtr sampler = si_->allocStateSampler(); 378 | 379 | while (true) 380 | { 381 | sampler->sampleUniformNear(state, near, distance); // samples (near + distance, near - distance) 382 | 383 | // Check if the sampled points are valid 384 | if( si_->isValid(state) ) 385 | { 386 | return; 387 | } 388 | else 389 | ROS_INFO_STREAM_NAMED("temp","Searching for valid start/goal state"); 390 | } 391 | } 392 | 393 | /** 394 | * \brief Show the planner data in Rviz 395 | * \param just_path - if true, do not display the search tree/graph or the samples 396 | */ 397 | void publishPlannerData(bool just_path) 398 | { 399 | // Final Solution ---------------------------------------------------------------- 400 | 401 | if (true) 402 | { 403 | // Show basic solution 404 | //visual_tools_->publishPath( simple_setup_->getSolutionPath(), RED); 405 | 406 | // Simplify solution 407 | //simple_setup_->simplifySolution(); 408 | 409 | // Interpolate solution 410 | simple_setup_->getSolutionPath().interpolate(); 411 | 412 | // Show path 413 | visual_tools_->publish2DPath( simple_setup_->getSolutionPath(), rviz_visual_tools::GREEN, 1.0, "final_solution"); 414 | } 415 | 416 | // Print the states to screen ----------------------------------------------------- 417 | if (false) 418 | { 419 | ROS_DEBUG_STREAM_NAMED("temp","showing path"); 420 | simple_setup_->getSolutionPath().print(std::cout); 421 | } 422 | 423 | // Get information about the exploration data structure the motion planner used in planning 424 | const ob::PlannerDataPtr planner_data( new ob::PlannerData( si_ ) ); 425 | simple_setup_->getPlannerData( *planner_data ); 426 | 427 | // Optionally display the search tree/graph or the samples 428 | if (!just_path) 429 | { 430 | // Visualize the explored space 431 | visual_tools_->publishGraph(planner_data, rviz_visual_tools::ORANGE, 0.2, "tree"); 432 | 433 | // Visualize the sample locations 434 | //visual_tools_->publishSamples(planner_data); 435 | } 436 | 437 | } 438 | 439 | /** \brief Allow access to simple_setup framework */ 440 | og::SimpleSetupPtr getSimpleSetup() 441 | { 442 | return simple_setup_; 443 | } 444 | 445 | 446 | }; // end of class 447 | 448 | } // namespace 449 | 450 | // ********************************************************************************************************* 451 | // Main 452 | // ********************************************************************************************************* 453 | int main( int argc, char** argv ) 454 | { 455 | ros::init(argc, argv, "ompl_visual_tools"); 456 | ROS_INFO( "OMPL Visual Tools Demo ----------------------------------------- " ); 457 | 458 | // Seed random 459 | srand ( time(NULL) ); 460 | 461 | // Allow the action server to recieve and send ros messages 462 | ros::AsyncSpinner spinner(1); 463 | spinner.start(); 464 | 465 | // Default argument values 466 | bool verbose = false; 467 | bool use_visuals = true; 468 | std::string image_path; 469 | std::size_t runs = 1; 470 | 471 | if (argc > 1) 472 | { 473 | for (std::size_t i = 0; i < std::size_t(argc); ++i) 474 | { 475 | // Help mode 476 | if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) 477 | { 478 | ROS_INFO_STREAM_NAMED("main","Usage: ompl_rviz_demos" 479 | << " --verbose --noVisuals --image [image_file] --runs [num plans] " 480 | << " -h --help"); 481 | return 0; 482 | } 483 | 484 | // Check for verbose flag 485 | if (strcmp(argv[i], "--verbose") == 0) 486 | { 487 | ROS_INFO_STREAM_NAMED("main","Running in VERBOSE mode (slower)"); 488 | verbose = true; 489 | } 490 | 491 | // Check if we should publish markers 492 | if (strcmp(argv[i], "--noVisuals") == 0) 493 | { 494 | ROS_INFO_STREAM_NAMED("main","NOT displaying graphics"); 495 | use_visuals = false; 496 | } 497 | 498 | // Check if user has passed in an image to read 499 | if (strcmp(argv[i], "--image") == 0) 500 | { 501 | i++; // get next arg 502 | image_path = argv[i]; 503 | } 504 | 505 | // Check if user has passed in the number of runs to perform 506 | if (strcmp(argv[i], "--runs") == 0) 507 | { 508 | i++; // get next arg 509 | runs = atoi( argv[i] ); 510 | } 511 | } 512 | } 513 | 514 | // Provide image if necessary 515 | if (image_path.empty()) // use default image 516 | { 517 | // Get image path based on package name 518 | image_path = ros::package::getPath("ompl_visual_tools"); 519 | if( image_path.empty() ) 520 | { 521 | ROS_ERROR( "Unable to get OMPL Visual Tools package path " ); 522 | return false; 523 | } 524 | 525 | // Choose random image 526 | int rand_num = ompl_visual_tools::OmplVisualTools::dRand(0,2); 527 | switch( rand_num ) 528 | { 529 | case 0: 530 | image_path.append( "/resources/wilbur_medium/wilbur_medium1.ppm" ); 531 | break; 532 | case 1: 533 | image_path.append( "/resources/wilbur_medium/wilbur_medium2.ppm" ); 534 | break; 535 | default: 536 | ROS_ERROR_STREAM_NAMED("main","Random has no case " << rand_num); 537 | break; 538 | } 539 | } 540 | 541 | // Create the planner 542 | ompl_visual_tools::RRTDemo demo(verbose, use_visuals); 543 | 544 | // Clear Rviz 545 | demo.deleteAllMakers(); 546 | 547 | // Load an image 548 | ROS_INFO_STREAM_NAMED("main","Loading image " << image_path); 549 | demo.loadCostMapImage( image_path, 0.4 ); 550 | demo.publishCostMapImage(); 551 | 552 | // Run the demo the desired number of times 553 | for (std::size_t i = 0; i < runs; ++i) 554 | { 555 | // Check if user wants to shutdown 556 | if (!ros::ok()) 557 | { 558 | ROS_WARN_STREAM_NAMED("plan","Terminating early"); 559 | break; 560 | } 561 | ROS_INFO_STREAM_NAMED("plan","Planning " << i+1 << " out of " << runs << " ------------------------------------"); 562 | 563 | // Refresh visuals 564 | if (use_visuals && i > 0) 565 | { 566 | demo.publishCostMapImage(); 567 | ros::spinOnce(); 568 | } 569 | 570 | // Run the planner 571 | demo.plan( i, runs ); 572 | 573 | // Create a pause if we are doing more runs and this is not the last run 574 | if (runs > 1 && i < runs - 1 && use_visuals) 575 | { 576 | // Let publisher publish 577 | ros::spinOnce(); 578 | ros::Duration(5.0).sleep(); 579 | } 580 | 581 | // Clear markers if this is not our last run 582 | if (i < runs - 1 && use_visuals) 583 | demo.deleteAllMakers(); 584 | } 585 | 586 | // Wait to let anything still being published finish 587 | ros::Duration(0.1).sleep(); 588 | 589 | ROS_INFO_STREAM("Shutting down."); 590 | 591 | return 0; 592 | } 593 | -------------------------------------------------------------------------------- /src/moveit_viz_window.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tools for displaying OMPL components in Rviz 37 | */ 38 | 39 | #include 40 | 41 | // ROS 42 | #include 43 | #include 44 | #include 45 | 46 | // OMPL 47 | #include 48 | #include 49 | 50 | // For converting OMPL state to a MoveIt robot state 51 | #include 52 | #include 53 | #include 54 | 55 | namespace ot = ompl::tools; 56 | namespace ob = ompl::base; 57 | namespace og = ompl::geometric; 58 | 59 | namespace ompl_visual_tools 60 | { 61 | MoveItVizWindow::MoveItVizWindow(moveit_visual_tools::MoveItVisualToolsPtr visuals, ompl::base::SpaceInformationPtr si) 62 | : name_("moveit_viz_window"), visuals_(visuals), si_(si) 63 | { 64 | // with this OMPL interface to Rviz all pubs must be manually triggered 65 | //visuals_->enableBatchPublishing(false); 66 | 67 | ROS_DEBUG_STREAM_NAMED(name_, "Initializing MoveItVizWindow()"); 68 | } 69 | 70 | void MoveItVizWindow::state(const ompl::base::State* state, ot::VizSizes size, ot::VizColors color, double extra_data) 71 | { 72 | // We do not use stateToPoint() because the publishRobotState() function might need the robot state in this function 73 | visuals_->loadSharedRobotState(); 74 | moveit_ompl::ModelBasedStateSpacePtr mb_state_space = 75 | std::static_pointer_cast(si_->getStateSpace()); 76 | // We must use the root_robot_state here so that the virtual_joint isn't affected 77 | mb_state_space->copyToRobotState(*visuals_->getRootRobotState(), state); 78 | Eigen::Affine3d pose = visuals_->getRootRobotState()->getGlobalLinkTransform("right_gripper_target"); 79 | 80 | switch (size) 81 | { 82 | case ompl::tools::SMALL: 83 | visuals_->publishSphere(pose, visuals_->intToRvizColor(color), rvt::SMALL); 84 | break; 85 | case ompl::tools::MEDIUM: 86 | visuals_->publishSphere(pose, visuals_->intToRvizColor(color), rvt::REGULAR); 87 | break; 88 | case ompl::tools::LARGE: 89 | visuals_->publishSphere(pose, visuals_->intToRvizColor(color), rvt::LARGE); 90 | break; 91 | case ompl::tools::VARIABLE_SIZE: // Medium purple, translucent outline 92 | // Visual tools has a scaling feature that will mess up the exact scaling we desire, so we out-smart it 93 | extra_data /= visuals_->getGlobalScale(); 94 | visuals_->publishSphere(visuals_->convertPose(pose), visuals_->intToRvizColor(color), extra_data * 2); 95 | break; 96 | case ompl::tools::SCALE: // Display sphere based on value between 0-100 97 | { 98 | const double percent = (extra_data - min_edge_cost_) / (max_edge_cost_ - min_edge_cost_); 99 | double radius = ((max_state_radius_ - min_state_radius_) * percent + min_state_radius_); 100 | 101 | // Visual tools has a scaling feature that will mess up the exact scaling we desire, so we out-smart it 102 | radius /= visuals_->getGlobalScale(); 103 | 104 | geometry_msgs::Vector3 scale; 105 | scale.x = radius; 106 | scale.y = radius; 107 | scale.z = radius; 108 | visuals_->publishSphere(pose, visuals_->getColorScale(percent), scale); 109 | } 110 | break; 111 | case ompl::tools::ROBOT: // Show actual robot in custom color 112 | mb_state_space->copyToRobotState(*visuals_->getSharedRobotState(), state); 113 | visuals_->publishRobotState(visuals_->getSharedRobotState(), visuals_->intToRvizColor(color)); 114 | break; 115 | default: 116 | ROS_ERROR_STREAM_NAMED(name_, "vizStateRobot: Invalid state type value"); 117 | } // end switch 118 | } 119 | 120 | void MoveItVizWindow::states(std::vector states, std::vector colors, 121 | ot::VizSizes size) 122 | { 123 | std::cout << "not implemented for mvoeit_viz_window " << std::endl; 124 | // // Cache spheres 125 | // EigenSTL::vector_Vector3d sphere_points; 126 | // std::vector sphere_colors; 127 | 128 | // for (std::size_t i = 0; i < states.size(); ++i) 129 | // { 130 | // // Convert OMPL state to vector3 131 | // sphere_points.push_back(stateToPoint(states[i])); 132 | // // Convert OMPL color to Rviz color 133 | // sphere_colors.push_back(visuals_->intToRvizColor(colors[i])); 134 | // } 135 | 136 | // // Publish 137 | // visuals_->publishSpheres(sphere_points, sphere_colors, visuals_->intToRvizScale(size)); 138 | } 139 | 140 | void MoveItVizWindow::edge(const ompl::base::State* stateA, const ompl::base::State* stateB, double cost) 141 | { 142 | // Error check 143 | if (si_->getStateSpace()->equalStates(stateA, stateB)) 144 | { 145 | ROS_WARN_STREAM_NAMED(name_, "Unable to visualize edge because states are the same"); 146 | visuals_->publishSphere(stateToPoint(stateA), rvt::RED, rvt::XLARGE); 147 | visuals_->triggerBatchPublish(); 148 | ros::Duration(0.01).sleep(); 149 | return; 150 | } 151 | 152 | // Convert input cost 153 | double percent = (cost - min_edge_cost_) / (max_edge_cost_ - min_edge_cost_); 154 | 155 | // Swap colors 156 | if (invert_edge_cost_) 157 | { 158 | percent = 1 - percent; 159 | } 160 | 161 | const double radius = (max_edge_radius_ - min_edge_radius_) * percent + min_edge_radius_; 162 | 163 | if (false) 164 | { 165 | std::cout << "cost: " << cost << " min_edge_cost_: " << min_edge_cost_ << " max_edge_cost_: " << max_edge_cost_ 166 | << " percent: " << percent << " radius: " << radius << std::endl; 167 | std::cout << "max edge r: " << max_edge_radius_ << " min edge r: " << min_edge_radius_ << std::endl; 168 | } 169 | 170 | visuals_->publishLine(stateToPoint(stateA), stateToPoint(stateB), visuals_->getColorScale(percent), radius / 2.0); 171 | } 172 | 173 | void MoveItVizWindow::edge(const ompl::base::State* stateA, const ompl::base::State* stateB, ot::VizSizes size, 174 | ot::VizColors color) 175 | { 176 | visuals_->publishCylinder(stateToPoint(stateA), stateToPoint(stateB), visuals_->intToRvizColor(color), 177 | visuals_->intToRvizScale(size)); 178 | } 179 | 180 | void MoveItVizWindow::path(ompl::geometric::PathGeometric* path, ompl::tools::VizSizes type, ot::VizColors color) 181 | { 182 | // Convert 183 | const og::PathGeometric& geometric_path = *path; // static_cast(*path); 184 | 185 | switch (type) 186 | { 187 | case ompl::tools::SMALL: // Basic line with vertiices 188 | publish2DPath(geometric_path, visuals_->intToRvizColor(color), min_edge_radius_); 189 | publishSpheres(geometric_path, visuals_->intToRvizColor(color), rvt::SMALL); 190 | break; 191 | case ompl::tools::MEDIUM: // Basic line with vertiices 192 | publish2DPath(geometric_path, visuals_->intToRvizColor(color), max_edge_radius_ / 2.0); 193 | publishSpheres(geometric_path, visuals_->intToRvizColor(color), rvt::SMALL); 194 | break; 195 | case ompl::tools::LARGE: // Basic line with vertiices 196 | publish2DPath(geometric_path, visuals_->intToRvizColor(color), max_edge_radius_); 197 | publishSpheres(geometric_path, visuals_->intToRvizColor(color), rvt::SMALL); 198 | break; 199 | case ompl::tools::ROBOT: // Playback motion for real robot 200 | // Check that jmg_ was set 201 | if (!jmg_) 202 | ROS_ERROR_STREAM_NAMED(name_, "Joint model group has not been set"); 203 | 204 | publishTrajectoryPath(geometric_path, jmg_, true /*wait_for_trajectory*/); 205 | break; 206 | default: 207 | ROS_ERROR_STREAM_NAMED(name_, "Invalid vizPath type value " << type); 208 | } 209 | } 210 | 211 | void MoveItVizWindow::trigger() 212 | { 213 | vizTrigger(); 214 | } 215 | 216 | void MoveItVizWindow::deleteAllMarkers() 217 | { 218 | visuals_->deleteAllMarkers(); 219 | } 220 | 221 | bool MoveItVizWindow::shutdownRequested() 222 | { 223 | if (!ros::ok()) 224 | { 225 | std::cout << "Shutting down process by request of ros::ok()" << std::endl; 226 | return true; 227 | } 228 | return false; 229 | } 230 | 231 | // From ompl_visual_tools ------------------------------------------------------ 232 | 233 | bool MoveItVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, double scale, 234 | const std::string& ns) 235 | { 236 | geometry_msgs::Vector3 scale_vector; 237 | scale_vector.x = scale; 238 | scale_vector.y = scale; 239 | scale_vector.z = scale; 240 | return publishSpheres(path, color, scale_vector, ns); 241 | } 242 | 243 | bool MoveItVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, const rvt::scales scale, 244 | const std::string& ns) 245 | { 246 | return publishSpheres(path, color, visuals_->getScale(scale), ns); 247 | } 248 | 249 | bool MoveItVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, 250 | const geometry_msgs::Vector3& scale, const std::string& ns) 251 | { 252 | std::vector points; 253 | for (std::size_t i = 0; i < path.getStateCount(); ++i) 254 | points.push_back(visuals_->convertPoint(stateToPoint(path.getState(i)))); 255 | 256 | return visuals_->publishSpheres(points, color, scale, ns); 257 | } 258 | 259 | // bool MoveItVizWindow::publishStates(std::vector states) 260 | // { 261 | // visualization_msgs::Marker marker; 262 | // // Set the frame ID and timestamp. 263 | // marker.header.frame_id = visuals_->getBaseFrame(); 264 | // marker.header.stamp = ros::Time(); 265 | 266 | // // Set the namespace and id for this marker. This serves to create a unique ID 267 | // marker.ns = "states"; 268 | 269 | // // Set the marker type. 270 | // marker.type = visualization_msgs::Marker::SPHERE_LIST; 271 | 272 | // // Set the marker action. Options are ADD and DELETE 273 | // marker.action = visualization_msgs::Marker::ADD; 274 | // marker.id = 0; 275 | 276 | // marker.pose.position.x = 0.0; 277 | // marker.pose.position.y = 0.0; 278 | // marker.pose.position.z = 0.0; 279 | 280 | // marker.pose.orientation.x = 0.0; 281 | // marker.pose.orientation.y = 0.0; 282 | // marker.pose.orientation.z = 0.0; 283 | // marker.pose.orientation.w = 1.0; 284 | 285 | // marker.scale.x = 0.4; 286 | // marker.scale.y = 0.4; 287 | // marker.scale.z = 0.4; 288 | 289 | // marker.color = visuals_->getColor(rvt::RED); 290 | 291 | // // Make line color 292 | // std_msgs::ColorRGBA color = visuals_->getColor(rvt::RED); 293 | 294 | // // Point 295 | // geometry_msgs::Point point_a; 296 | 297 | // // Loop through all verticies 298 | // for (int vertex_id = 0; vertex_id < int(states.size()); ++vertex_id) 299 | // { 300 | // // First point 301 | // point_a = visuals_->convertPoint(stateToPoint(states[vertex_id])); 302 | 303 | // // Add the point pair to the line message 304 | // marker.points.push_back(point_a); 305 | // marker.colors.push_back(color); 306 | // } 307 | 308 | // // Send to Rviz 309 | // return visuals_->publishMarker(marker); 310 | // } 311 | 312 | bool MoveItVizWindow::publishRobotState(const ompl::base::State* state) 313 | { 314 | // Make sure a robot state is available 315 | visuals_->loadSharedRobotState(); 316 | 317 | moveit_ompl::ModelBasedStateSpacePtr mb_state_space = 318 | std::static_pointer_cast(si_->getStateSpace()); 319 | 320 | // Convert to robot state 321 | mb_state_space->copyToRobotState(*visuals_->getSharedRobotState(), state); 322 | 323 | // Show the robot visualized in Rviz 324 | return visuals_->publishRobotState(visuals_->getSharedRobotState()); 325 | } 326 | 327 | bool MoveItVizWindow::publishTrajectoryPath(const ompl::base::PlannerDataPtr& path, robot_model::JointModelGroup* jmg, 328 | const std::vector& tips, 329 | bool show_trajectory_animated) 330 | { 331 | // Make sure a robot state is available 332 | visuals_->loadSharedRobotState(); 333 | 334 | // Vars 335 | Eigen::Affine3d pose; 336 | std::vector > paths_msgs(tips.size()); // each tip has its own path of points 337 | robot_trajectory::RobotTrajectoryPtr robot_trajectory; 338 | 339 | moveit_ompl::ModelBasedStateSpacePtr mb_state_space = 340 | std::static_pointer_cast(si_->getStateSpace()); 341 | 342 | // Optionally save the trajectory 343 | if (show_trajectory_animated) 344 | { 345 | robot_trajectory.reset(new robot_trajectory::RobotTrajectory(visuals_->getRobotModel(), jmg->getName())); 346 | } 347 | 348 | // Each state in the path 349 | for (std::size_t state_id = 0; state_id < path->numVertices(); ++state_id) 350 | { 351 | // Check if program is shutting down 352 | if (!ros::ok()) 353 | return false; 354 | 355 | // Convert to robot state 356 | mb_state_space->copyToRobotState(*visuals_->getSharedRobotState(), path->getVertex(state_id).getState()); 357 | ROS_WARN_STREAM_NAMED("temp", "updateStateWithFakeBase disabled"); 358 | // visuals_->getSharedRobotState()->updateStateWithFakeBase(); 359 | 360 | visuals_->publishRobotState(visuals_->getSharedRobotState()); 361 | 362 | // Each tip in the robot state 363 | for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id) 364 | { 365 | // Forward kinematics 366 | pose = visuals_->getSharedRobotState()->getGlobalLinkTransform(tips[tip_id]); 367 | 368 | // Optionally save the trajectory 369 | if (show_trajectory_animated) 370 | { 371 | robot_state::RobotState robot_state_copy = *visuals_->getSharedRobotState(); 372 | robot_trajectory->addSuffixWayPoint(robot_state_copy, 0.01); // 1 second interval 373 | } 374 | 375 | // Debug pose 376 | // std::cout << "Pose: " << state_id << " of link " << tips[tip_id]->getName() << ": \n" << pose.translation() << 377 | // std::endl; 378 | 379 | paths_msgs[tip_id].push_back(visuals_->convertPose(pose).position); 380 | 381 | // Show goal state arrow 382 | if (state_id == path->numVertices() - 1) 383 | { 384 | visuals_->publishArrow(pose, rvt::BLACK); 385 | } 386 | } 387 | } // for each state 388 | 389 | for (std::size_t tip_id = 0; tip_id < tips.size(); ++tip_id) 390 | { 391 | visuals_->publishPath(paths_msgs[tip_id], rvt::RAND, rvt::SMALL); 392 | ros::Duration(0.05).sleep(); 393 | visuals_->publishSpheres(paths_msgs[tip_id], rvt::ORANGE, rvt::SMALL); 394 | ros::Duration(0.05).sleep(); 395 | } 396 | 397 | // Debugging - Convert to trajectory 398 | if (show_trajectory_animated) 399 | { 400 | visuals_->publishTrajectoryPath(*robot_trajectory, true); 401 | } 402 | 403 | return true; 404 | } 405 | 406 | bool MoveItVizWindow::publishTrajectoryPath(const og::PathGeometric& path, const robot_model::JointModelGroup* jmg, 407 | const bool blocking) 408 | { 409 | // Convert to MoveIt! format 410 | robot_trajectory::RobotTrajectoryPtr traj; 411 | double speed = 0.01; 412 | if (!convertPath(path, jmg, traj, speed)) 413 | { 414 | return false; 415 | } 416 | 417 | return visuals_->publishTrajectoryPath(*traj, blocking); 418 | } 419 | 420 | // Deprecated 421 | bool MoveItVizWindow::publishPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness, 422 | const std::string& ns) 423 | { 424 | return publish2DPath(path, color, thickness, ns); 425 | } 426 | 427 | bool MoveItVizWindow::publish2DPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness, 428 | const std::string& ns) 429 | { 430 | // Error check 431 | if (path.getStateCount() <= 0) 432 | { 433 | ROS_WARN_STREAM_NAMED(name_, "No states found in path"); 434 | return false; 435 | } 436 | 437 | // Initialize first vertex 438 | Eigen::Vector3d prev_vertex = stateToPoint(path.getState(0)); 439 | Eigen::Vector3d this_vertex; 440 | 441 | // Convert path coordinates 442 | for (std::size_t i = 1; i < path.getStateCount(); ++i) 443 | { 444 | // Get current coordinates 445 | this_vertex = stateToPoint(path.getState(i)); 446 | 447 | // Create line 448 | visuals_->publishCylinder(prev_vertex, this_vertex, color, thickness, ns); 449 | 450 | // Save these coordinates for next line 451 | prev_vertex = this_vertex; 452 | } 453 | 454 | return true; 455 | } 456 | 457 | Eigen::Vector3d MoveItVizWindow::stateToPoint(const ob::ScopedState<> state) 458 | { 459 | return stateToPoint(state.get()); 460 | } 461 | 462 | Eigen::Vector3d MoveItVizWindow::stateToPoint(const ob::State* state) 463 | { 464 | if (!state) 465 | { 466 | ROS_FATAL_NAMED(name_, "No state found for vertex"); 467 | exit(1); 468 | } 469 | 470 | // Make sure a robot state is available 471 | visuals_->loadSharedRobotState(); 472 | 473 | moveit_ompl::ModelBasedStateSpacePtr mb_state_space = 474 | std::static_pointer_cast(si_->getStateSpace()); 475 | 476 | // Convert to robot state 477 | mb_state_space->copyToRobotState(*visuals_->getRootRobotState(), state); 478 | 479 | // Get pose 480 | // TODO(davetcoleman): do not hard code 481 | Eigen::Affine3d pose = visuals_->getRootRobotState()->getGlobalLinkTransform("right_gripper_target"); 482 | 483 | return pose.translation(); 484 | } 485 | 486 | bool MoveItVizWindow::publishState(const ob::State* state, const rvt::colors& color, const rvt::scales scale, 487 | const std::string& ns) 488 | { 489 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 490 | } 491 | 492 | bool MoveItVizWindow::publishState(const ob::State* state, const rvt::colors& color, const double scale, 493 | const std::string& ns) 494 | { 495 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 496 | } 497 | 498 | bool MoveItVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, const rvt::scales scale, 499 | const std::string& ns) 500 | { 501 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 502 | } 503 | 504 | bool MoveItVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, double scale, 505 | const std::string& ns) 506 | { 507 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 508 | } 509 | 510 | bool MoveItVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, 511 | const geometry_msgs::Vector3& scale, const std::string& ns) 512 | { 513 | return visuals_->publishSphere(stateToPoint(state), color, scale.x, ns); 514 | } 515 | 516 | bool MoveItVizWindow::publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance) 517 | { 518 | temp_point_.x = state_area[0]; 519 | temp_point_.y = state_area[1]; 520 | temp_point_.z = state_area[2]; 521 | 522 | visuals_->publishSphere(temp_point_, rvt::BLACK, rvt::REGULAR, "sample_region"); // mid point 523 | // outer sphere (x2 b/c its a radius, x0.1 to make it look nicer) 524 | return visuals_->publishSphere(temp_point_, rvt::TRANSLUCENT, rvt::REGULAR, "sample_region"); 525 | } 526 | 527 | bool MoveItVizWindow::convertPath(const og::PathGeometric& path, const robot_model::JointModelGroup* jmg, 528 | robot_trajectory::RobotTrajectoryPtr& traj, double speed) 529 | { 530 | // Error check 531 | if (path.getStateCount() <= 0) 532 | { 533 | ROS_WARN_STREAM_NAMED(name_, "No states found in path"); 534 | return false; 535 | } 536 | 537 | // New trajectory 538 | traj.reset(new robot_trajectory::RobotTrajectory(visuals_->getRobotModel(), jmg)); 539 | moveit::core::RobotState state(*visuals_->getSharedRobotState()); // TODO(davetcoleman):do i need to copy this? 540 | 541 | // Get correct type of space 542 | moveit_ompl::ModelBasedStateSpacePtr mb_state_space = 543 | std::static_pointer_cast(si_->getStateSpace()); 544 | 545 | // Convert solution to MoveIt! format, reversing the solution 546 | // for (std::size_t i = path.getStateCount(); i > 0; --i) 547 | for (std::size_t i = 0; i < path.getStateCount(); ++i) 548 | { 549 | // Convert format 550 | mb_state_space->copyToRobotState(state, path.getState(i)); 551 | 552 | // Add to trajectory 553 | traj->addSuffixWayPoint(state, speed); 554 | } 555 | return true; 556 | } 557 | 558 | void MoveItVizWindow::vizTrigger() 559 | { 560 | visuals_->triggerBatchPublish(); 561 | 562 | // Kill OMPL 563 | // if (!ros::ok()) 564 | // { 565 | // std::cout << std::endl; 566 | // std::cout << "-------------------------------------------------------" << std::endl; 567 | // std::cout << "Shutting down process by request of ros::ok()" << std::endl; 568 | // std::cout << "-------------------------------------------------------" << std::endl; 569 | // exit(0); 570 | // } 571 | } 572 | 573 | 574 | } // namespace ompl_visual_tools 575 | -------------------------------------------------------------------------------- /src/projection_viz_window.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tools for displaying OMPL components in Rviz 37 | */ 38 | 39 | #include 40 | 41 | // ROS 42 | #include 43 | #include 44 | #include 45 | 46 | // Boost 47 | #include 48 | #include 49 | 50 | // OMPL 51 | #include 52 | #include 53 | 54 | // Custom validity checker that accounts for cost 55 | #include 56 | 57 | // MoveIt 58 | #include 59 | 60 | namespace ot = ompl::tools; 61 | namespace ob = ompl::base; 62 | namespace og = ompl::geometric; 63 | namespace bnu = boost::numeric::ublas; 64 | 65 | namespace ompl_visual_tools 66 | { 67 | ProjectionVizWindow::ProjectionVizWindow(rviz_visual_tools::RvizVisualToolsPtr visuals, 68 | ompl::base::SpaceInformationPtr si) 69 | : name_("projection_viz_window"), visuals_(visuals), si_(si) 70 | { 71 | // with this OMPL interface to Rviz all pubs must be manually triggered 72 | //visuals_->enableBatchPublishing(false); 73 | 74 | // Calculate ranges 75 | moveit_ompl::ModelBasedStateSpacePtr mb_state_space = 76 | std::static_pointer_cast(si_->getStateSpace()); 77 | ompl::base::RealVectorBounds bounds = mb_state_space->getBounds(); 78 | 79 | // Only allow up to 6 dimensions 80 | BOOST_ASSERT_MSG(si_->getStateSpace()->getDimension() > 0 && si_->getStateSpace()->getDimension() <= 6, 81 | "Invalid number of dimensions"); 82 | 83 | // For each dimension 84 | for (std::size_t i = 0; i < si_->getStateSpace()->getDimension(); ++i) 85 | { 86 | range_.push_back(fabs(bounds.high[i] - bounds.low[i])); 87 | low_.push_back(bounds.low[i]); 88 | BOOST_ASSERT_MSG(range_.back() > 0, "Range is zero"); 89 | } 90 | 91 | ROS_DEBUG_STREAM_NAMED(name_, "Initializing ProjectionVizWindow()"); 92 | } 93 | 94 | void ProjectionVizWindow::state(const ompl::base::State* state, ot::VizSizes size, ot::VizColors color, 95 | double extra_data) 96 | { 97 | Eigen::Vector3d point2 = stateToPoint(state); 98 | 99 | switch (size) 100 | { 101 | case ompl::tools::VARIABLE_SIZE: 102 | 103 | //extra_data /= range_[0]; // hack for projection TODO(davetcoleman): is this correct? 104 | std::cout << "orig extra_data " << extra_data << std::endl; 105 | extra_data = (extra_data - low_[0]) / range_[0]; 106 | std::cout << "projected extra_data " << extra_data << std::endl; 107 | extra_data /= visuals_->getGlobalScale(); 108 | std::cout << "reverse scaled extra_data " << extra_data << std::endl; 109 | 110 | visuals_->publishSphere(point2, visuals_->intToRvizColor(color), extra_data); 111 | break; 112 | case ompl::tools::SCALE: 113 | { 114 | const double percent = (extra_data - min_edge_cost_) / (max_edge_cost_ - min_edge_cost_); 115 | const double radius = ((max_state_radius_ - min_state_radius_) * percent + min_state_radius_); 116 | geometry_msgs::Vector3 scale; 117 | scale.x = radius; 118 | scale.y = radius; 119 | scale.z = radius; 120 | visuals_->publishSphere(visuals_->convertPointToPose(point2), visuals_->getColorScale(percent), scale); 121 | } 122 | break; 123 | default: 124 | visuals_->publishSphere(point2, visuals_->intToRvizColor(color), visuals_->intToRvizScale(size)); 125 | } // switch 126 | } 127 | 128 | void ProjectionVizWindow::states(std::vector states, std::vector colors, 129 | ot::VizSizes size) 130 | { 131 | // Cache spheres 132 | EigenSTL::vector_Vector3d sphere_points; 133 | std::vector sphere_colors; 134 | 135 | for (std::size_t i = 0; i < states.size(); ++i) 136 | { 137 | // Convert OMPL state to vector3 138 | sphere_points.push_back(stateToPoint(states[i])); 139 | // Convert OMPL color to Rviz color 140 | sphere_colors.push_back(visuals_->intToRvizColor(colors[i])); 141 | } 142 | 143 | // Publish 144 | visuals_->publishSpheres(sphere_points, sphere_colors, visuals_->intToRvizScale(size)); 145 | } 146 | 147 | void ProjectionVizWindow::edge(const ompl::base::State* stateA, const ompl::base::State* stateB, double cost) 148 | { 149 | // Error check 150 | if (si_->getStateSpace()->equalStates(stateA, stateB)) 151 | { 152 | ROS_WARN_STREAM_NAMED(name_, "Unable to visualize edge because states are the same"); 153 | visuals_->publishSphere(stateToPoint(stateA), rvt::RED, rvt::XLARGE); 154 | visuals_->triggerBatchPublish(); 155 | ros::Duration(0.01).sleep(); 156 | return; 157 | } 158 | 159 | // Convert input cost 160 | double percent = (cost - min_edge_cost_) / (max_edge_cost_ - min_edge_cost_); 161 | 162 | // Swap colors 163 | if (invert_edge_cost_) 164 | { 165 | percent = 1 - percent; 166 | } 167 | 168 | // const double radius = percent / 6.0 + 0.15; 169 | const double radius = (max_edge_radius_ - min_edge_radius_) * percent + min_edge_radius_; 170 | 171 | if (false) 172 | std::cout << "cost: " << cost << " min_edge_cost_: " << min_edge_cost_ << " max_edge_cost_: " << max_edge_cost_ 173 | << " percent: " << percent << " radius: " << radius << std::endl; 174 | 175 | publishEdge(stateA, stateB, visuals_->getColorScale(percent), radius); 176 | } 177 | 178 | void ProjectionVizWindow::edge(const ompl::base::State* stateA, const ompl::base::State* stateB, ot::VizSizes size, 179 | ot::VizColors color) 180 | { 181 | visuals_->publishCylinder(stateToPoint(stateA), stateToPoint(stateB), visuals_->intToRvizColor(color), 182 | visuals_->intToRvizScale(size)); 183 | } 184 | 185 | void ProjectionVizWindow::edges(const std::vector stateAs, 186 | const std::vector stateBs, 187 | std::vector colors, ompl::tools::VizSizes size) 188 | { 189 | // Cache edges 190 | EigenSTL::vector_Vector3d aPoints; 191 | EigenSTL::vector_Vector3d bPoints; 192 | std::vector rviz_colors; 193 | 194 | for (std::size_t i = 0; i < stateAs.size(); ++i) 195 | { 196 | // Convert OMPL states to vector3 197 | aPoints.push_back(stateToPoint(stateAs[i])); 198 | bPoints.push_back(stateToPoint(stateBs[i])); 199 | 200 | // Convert OMPL color to Rviz color 201 | rviz_colors.push_back(visuals_->intToRvizColor(colors[i])); 202 | } 203 | 204 | // Publish 205 | visuals_->publishLines(aPoints, bPoints, rviz_colors, visuals_->intToRvizScale(size)); 206 | } 207 | 208 | void ProjectionVizWindow::path(ompl::geometric::PathGeometric* path, ompl::tools::VizSizes type, ot::VizColors color) 209 | { 210 | // Convert 211 | const og::PathGeometric& geometric_path = *path; // static_cast(*path); 212 | 213 | switch (type) 214 | { 215 | case ompl::tools::SMALL: // Basic black line with vertiices 216 | publish2DPath(geometric_path, visuals_->intToRvizColor(color), min_edge_radius_); 217 | publishSpheres(geometric_path, visuals_->intToRvizColor(color), rvt::SMALL); 218 | break; 219 | case ompl::tools::ROBOT: 220 | // Playback motion for real robot, which is not applicable for this space 221 | break; 222 | default: 223 | ROS_ERROR_STREAM_NAMED(name_, "Invalid vizPath type value " << type); 224 | } 225 | } 226 | 227 | void ProjectionVizWindow::trigger() 228 | { 229 | visuals_->triggerBatchPublish(); 230 | } 231 | 232 | void ProjectionVizWindow::deleteAllMarkers() 233 | { 234 | visuals_->deleteAllMarkers(); 235 | } 236 | 237 | bool ProjectionVizWindow::shutdownRequested() 238 | { 239 | if (!ros::ok()) 240 | { 241 | std::cout << "Shutting down process by request of ros::ok()" << std::endl; 242 | return true; 243 | } 244 | return false; 245 | } 246 | 247 | // From ompl_visual_tools ------------------------------------------------------ 248 | 249 | bool ProjectionVizWindow::publishEdge(const ob::State* stateA, const ob::State* stateB, 250 | const std_msgs::ColorRGBA& color, const double radius) 251 | { 252 | return visuals_->publishCylinder(stateToPoint(stateA), stateToPoint(stateB), color, radius / 2.0); 253 | } 254 | 255 | bool ProjectionVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, double scale, 256 | const std::string& ns) 257 | { 258 | geometry_msgs::Vector3 scale_vector; 259 | scale_vector.x = scale; 260 | scale_vector.y = scale; 261 | scale_vector.z = scale; 262 | return publishSpheres(path, color, scale_vector, ns); 263 | } 264 | 265 | bool ProjectionVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, 266 | const rvt::scales scale, const std::string& ns) 267 | { 268 | return publishSpheres(path, color, visuals_->getScale(scale), ns); 269 | } 270 | 271 | bool ProjectionVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, 272 | const geometry_msgs::Vector3& scale, const std::string& ns) 273 | { 274 | std::vector points; 275 | for (std::size_t i = 0; i < path.getStateCount(); ++i) 276 | points.push_back(visuals_->convertPoint(stateToPoint(path.getState(i)))); 277 | 278 | return visuals_->publishSpheres(points, color, scale, ns); 279 | } 280 | 281 | // Deprecated 282 | bool ProjectionVizWindow::publishPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness, 283 | const std::string& ns) 284 | { 285 | return publish2DPath(path, color, thickness, ns); 286 | } 287 | 288 | bool ProjectionVizWindow::publish2DPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness, 289 | const std::string& ns) 290 | { 291 | // Error check 292 | if (path.getStateCount() <= 0) 293 | { 294 | ROS_WARN_STREAM_NAMED(name_, "No states found in path"); 295 | return false; 296 | } 297 | 298 | // Initialize first vertex 299 | Eigen::Vector3d prev_vertex = stateToPoint(path.getState(0)); 300 | Eigen::Vector3d this_vertex; 301 | 302 | // Convert path coordinates 303 | for (std::size_t i = 1; i < path.getStateCount(); ++i) 304 | { 305 | // Get current coordinates 306 | this_vertex = stateToPoint(path.getState(i)); 307 | 308 | // Create line 309 | visuals_->publishCylinder(prev_vertex, this_vertex, color, thickness, ns); 310 | 311 | // Save these coordinates for next line 312 | prev_vertex = this_vertex; 313 | } 314 | 315 | return true; 316 | } 317 | 318 | int ProjectionVizWindow::natRound(double x) 319 | { 320 | return static_cast(floor(x + 0.5f)); 321 | } 322 | 323 | bool ProjectionVizWindow::publishState(const ob::State* state, const rvt::colors& color, const rvt::scales scale, 324 | const std::string& ns) 325 | { 326 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 327 | } 328 | 329 | bool ProjectionVizWindow::publishState(const ob::State* state, const rvt::colors& color, const double scale, 330 | const std::string& ns) 331 | { 332 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 333 | } 334 | 335 | bool ProjectionVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, const rvt::scales scale, 336 | const std::string& ns) 337 | { 338 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 339 | } 340 | 341 | bool ProjectionVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, double scale, 342 | const std::string& ns) 343 | { 344 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 345 | } 346 | 347 | bool ProjectionVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, 348 | const geometry_msgs::Vector3& scale, const std::string& ns) 349 | { 350 | return visuals_->publishSphere(stateToPoint(state), color, scale.x, ns); 351 | } 352 | 353 | bool ProjectionVizWindow::publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance) 354 | { 355 | temp_point_.x = state_area[0]; 356 | temp_point_.y = state_area[1]; 357 | temp_point_.z = state_area[2]; 358 | 359 | visuals_->publishSphere(temp_point_, rvt::BLACK, rvt::REGULAR, "sample_region"); // mid point 360 | // outer sphere (x2 b/c its a radius, x0.1 to make it look nicer) 361 | return visuals_->publishSphere(temp_point_, rvt::TRANSLUCENT, rvt::REGULAR, "sample_region"); 362 | } 363 | 364 | Eigen::Vector3d ProjectionVizWindow::stateToPoint(const ob::ScopedState<> state) 365 | { 366 | return stateToPoint(state.get()); 367 | } 368 | 369 | Eigen::Vector3d ProjectionVizWindow::stateToPoint(const ob::State* state) 370 | { 371 | if (!state) 372 | { 373 | ROS_FATAL_NAMED(name_, "No state found for vertex"); 374 | exit(1); 375 | } 376 | 377 | std::vector point(3 /*size*/, 0 /*default value*/); 378 | double temp; 379 | 380 | // For each dimension 381 | for (std::size_t i = 0; i < si_->getStateSpace()->getDimension(); ++i) 382 | { 383 | temp = state->as()->values[i]; 384 | 385 | if (i < 3) // regular dimensions 386 | { 387 | // Project to 1:1:1 3D space 388 | point[i] = (temp - low_[i]) / range_[i]; 389 | } 390 | else if (i >= 3) // Move the 3D space over 391 | { 392 | // Project to 1:1:1 3D space 393 | temp = (temp - low_[i]) / range_[i]; 394 | 395 | point[i-3] += temp * 5; 396 | } 397 | } 398 | 399 | // Copy to eigen structure 400 | //const double x_offset = 1.0; // Move all points to a hard coded location 401 | static const double x_offset = -0.5; // Move all points to a hard coded location 402 | static const double y_offset = -0.5; 403 | static const double z_offset = 0.0; 404 | temp_eigen_point_.x() = point[0] + x_offset; 405 | temp_eigen_point_.y() = point[1] + y_offset; 406 | temp_eigen_point_.z() = point[2] + z_offset; 407 | 408 | return temp_eigen_point_; 409 | } 410 | 411 | } // namespace ompl_visual_tools 412 | -------------------------------------------------------------------------------- /src/ros_viz_window.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Tools for displaying OMPL components in Rviz 37 | */ 38 | 39 | #include 40 | 41 | // ROS 42 | #include 43 | #include 44 | #include 45 | 46 | // Boost 47 | #include 48 | #include 49 | 50 | // OMPL 51 | #include 52 | #include 53 | 54 | // Custom validity checker that accounts for cost 55 | #include 56 | 57 | #include 58 | 59 | namespace ot = ompl::tools; 60 | namespace ob = ompl::base; 61 | namespace og = ompl::geometric; 62 | namespace bnu = boost::numeric::ublas; 63 | 64 | namespace ompl_visual_tools 65 | { 66 | ROSVizWindow::ROSVizWindow(rviz_visual_tools::RvizVisualToolsPtr visuals, ompl::base::SpaceInformationPtr si) 67 | : name_("ros_viz_window"), visuals_(visuals), si_(si) 68 | { 69 | // with this OMPL interface to Rviz all pubs must be manually triggered 70 | // visuals_->enableBatchPublishing(true); 71 | 72 | ROS_DEBUG_STREAM_NAMED(name_, "Initializing ROSVizWindow()"); 73 | } 74 | 75 | void ROSVizWindow::state(const ompl::base::State* state, ot::VizSizes size, ot::VizColors color, double extra_data) 76 | { 77 | Eigen::Vector3d point = stateToPoint(state); 78 | 79 | // Optional - show state above lines - looks good in 2D but not 3D TODO(davetcoleman): how to auto choose this 80 | if (si_->getStateDimension() == 2) 81 | point.z() += 0.5; 82 | 83 | switch (size) 84 | { 85 | case ompl::tools::XXSMALL: 86 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::XXXSMALL); 87 | break; 88 | case ompl::tools::XSMALL: 89 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::XXSMALL); 90 | break; 91 | case ompl::tools::SMALL: 92 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::XSMALL); 93 | break; 94 | case ompl::tools::MEDIUM: 95 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::SMALL); 96 | break; 97 | case ompl::tools::LARGE: 98 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::MEDIUM); 99 | break; 100 | case ompl::tools::XLARGE: 101 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::LARGE); 102 | break; 103 | case ompl::tools::XXLARGE: 104 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), rvt::XLARGE); 105 | break; 106 | case ompl::tools::VARIABLE_SIZE: 107 | // Visual tools has a scaling feature that will mess up the exact scaling we desire, so we out-smart it 108 | extra_data /= visuals_->getGlobalScale(); 109 | visuals_->publishSphere(point, visuals_->intToRvizColor(color), extra_data * 2); 110 | break; 111 | case ompl::tools::SCALE: 112 | { 113 | const double percent = (extra_data - min_edge_cost_) / (max_edge_cost_ - min_edge_cost_); 114 | double radius = ((max_state_radius_ - min_state_radius_) * percent + min_state_radius_); 115 | 116 | // Visual tools has a scaling feature that will mess up the exact scaling we desire, so we out-smart it 117 | radius /= visuals_->getGlobalScale(); 118 | 119 | geometry_msgs::Vector3 scale; 120 | scale.x = radius; 121 | scale.y = radius; 122 | scale.z = radius; 123 | visuals_->publishSphere(visuals_->convertPointToPose(point), visuals_->getColorScale(percent), scale); 124 | } 125 | break; 126 | case ompl::tools::SMALL_TRANSLUCENT: 127 | OMPL_WARN("SMALL_TRANSLUCENT in ros_viz_window... I think this should be removed"); 128 | visuals_->publishSphere(point, rvt::TRANSLUCENT_LIGHT, extra_data); 129 | break; 130 | case ompl::tools::ROBOT: 131 | std::cout << "skipping visualizing robot state " << std::endl; 132 | break; 133 | default: 134 | ROS_ERROR_STREAM_NAMED(name_, "vizState2D: Invalid state size value " << size); 135 | } 136 | } 137 | 138 | void ROSVizWindow::states(std::vector states, std::vector colors, 139 | ot::VizSizes size) 140 | { 141 | // Cache spheres 142 | EigenSTL::vector_Vector3d sphere_points; 143 | std::vector sphere_colors; 144 | 145 | for (std::size_t i = 0; i < states.size(); ++i) 146 | { 147 | // Convert OMPL state to vector3 148 | Eigen::Vector3d point = stateToPoint(states[i]); 149 | 150 | // Optional - show state above lines - looks good in 2D but not 3D TODO(davetcoleman): how to auto choose this 151 | if (si_->getStateDimension() == 2) 152 | point.z() += 0.5; 153 | 154 | sphere_points.push_back(point); 155 | 156 | // Convert OMPL color to Rviz color 157 | sphere_colors.push_back(visuals_->intToRvizColor(colors[i])); 158 | } 159 | 160 | // Publish - ROSVizWindow scales are one smaller than MoveItVizWindow 161 | visuals_->publishSpheres(sphere_points, sphere_colors, visuals_->intToRvizScale(size - 1)); 162 | } 163 | 164 | void ROSVizWindow::edge(const ompl::base::State* stateA, const ompl::base::State* stateB, ot::VizSizes size, 165 | ot::VizColors color) 166 | { 167 | Eigen::Vector3d pointA = stateToPoint(stateA); 168 | Eigen::Vector3d pointB = stateToPoint(stateB); 169 | 170 | double radius; 171 | switch (size) 172 | { 173 | case ompl::tools::SMALL: 174 | radius = 0.1; 175 | break; 176 | case ompl::tools::MEDIUM: 177 | radius = 0.2; 178 | break; 179 | case ompl::tools::LARGE: 180 | radius = 0.5; 181 | break; 182 | default: 183 | OMPL_ERROR("Unknown size"); 184 | exit(-1); 185 | } 186 | 187 | visuals_->publishCylinder(pointA, pointB, visuals_->intToRvizColor(color), radius); 188 | } 189 | 190 | void ROSVizWindow::edge(const ompl::base::State* stateA, const ompl::base::State* stateB, double cost) 191 | { 192 | // Error check 193 | if (si_->getStateSpace()->equalStates(stateA, stateB)) 194 | { 195 | ROS_WARN_STREAM_NAMED(name_, "Unable to visualize edge because states are the same"); 196 | visuals_->publishSphere(stateToPoint(stateA), rvt::RED, rvt::XLARGE); 197 | visuals_->triggerBatchPublish(); 198 | ros::Duration(0.01).sleep(); 199 | return; 200 | } 201 | 202 | // Convert input cost 203 | double percent = (cost - min_edge_cost_) / (max_edge_cost_ - min_edge_cost_); 204 | 205 | // Swap colors 206 | if (invert_edge_cost_) 207 | { 208 | percent = 1 - percent; 209 | } 210 | 211 | // const double radius = percent / 6.0 + 0.15; 212 | const double radius = (max_edge_radius_ - min_edge_radius_) * percent + min_edge_radius_; 213 | 214 | if (false) 215 | std::cout << "cost: " << cost << " min_edge_cost_: " << min_edge_cost_ << " max_edge_cost_: " << max_edge_cost_ 216 | << " percent: " << percent << " radius: " << radius << std::endl; 217 | 218 | publishEdge(stateA, stateB, visuals_->getColorScale(percent), radius); 219 | } 220 | 221 | void ROSVizWindow::path(ompl::geometric::PathGeometric* path, ompl::tools::VizSizes type, ot::VizColors color) 222 | { 223 | // Convert 224 | const og::PathGeometric& geometric_path = *path; // static_cast(*path); 225 | 226 | switch (type) 227 | { 228 | case ompl::tools::SMALL: // Basic black line with vertiices 229 | publish2DPath(geometric_path, visuals_->intToRvizColor(color), min_edge_radius_); 230 | publishSpheres(geometric_path, visuals_->intToRvizColor(color), rvt::SMALL); 231 | break; 232 | case ompl::tools::ROBOT: 233 | // Playback motion for real robot, which is not applicable for this space 234 | break; 235 | default: 236 | ROS_ERROR_STREAM_NAMED(name_, "Invalid vizPath type value " << type); 237 | } 238 | } 239 | 240 | void ROSVizWindow::trigger() 241 | { 242 | visuals_->triggerBatchPublish(); 243 | } 244 | 245 | void ROSVizWindow::deleteAllMarkers() 246 | { 247 | visuals_->deleteAllMarkers(); 248 | } 249 | 250 | bool ROSVizWindow::shutdownRequested() 251 | { 252 | if (!ros::ok()) 253 | { 254 | std::cout << "Shutting down process by request of ros::ok()" << std::endl; 255 | return true; 256 | } 257 | return false; 258 | } 259 | 260 | // From ompl_visual_tools ------------------------------------------------------ 261 | 262 | void ROSVizWindow::setCostMap(intMatrixPtr cost) 263 | { 264 | cost_ = cost; 265 | } 266 | 267 | double ROSVizWindow::getCost(const geometry_msgs::Point& point) 268 | { 269 | // Check that a cost map has been passed in 270 | if (cost_) 271 | { 272 | return double((*cost_)(natRound(point.y), natRound(point.x))) / 2.0; 273 | } 274 | else 275 | return 1; 276 | } 277 | 278 | bool ROSVizWindow::publishCostMap(PPMImage* image, bool static_id) 279 | { 280 | visualization_msgs::Marker marker; 281 | // Set the frame ID and timestamp. See the TF tutorials for information on these. 282 | marker.header.frame_id = visuals_->getBaseFrame(); 283 | marker.header.stamp = ros::Time::now(); 284 | 285 | // Set the namespace and id for this marker. This serves to create a unique ID 286 | marker.ns = "cost_map"; 287 | 288 | // Set the marker type. 289 | marker.type = visualization_msgs::Marker::TRIANGLE_LIST; 290 | 291 | // Set the marker action. Options are ADD and DELETE 292 | marker.action = visualization_msgs::Marker::ADD; 293 | 294 | static std::size_t cost_map_id = 0; 295 | if (static_id) 296 | { 297 | marker.id = 0; 298 | } 299 | else 300 | { 301 | cost_map_id++; 302 | marker.id = cost_map_id; 303 | } 304 | 305 | marker.pose.position.x = 0; 306 | marker.pose.position.y = 0; 307 | marker.pose.position.z = -0.25; 308 | marker.pose.orientation.x = 0.0; 309 | marker.pose.orientation.y = 0.0; 310 | marker.pose.orientation.z = 0.0; 311 | marker.pose.orientation.w = 1.0; 312 | marker.scale.x = 1.0; 313 | marker.scale.y = 1.0; 314 | marker.scale.z = 1.0; 315 | marker.color = visuals_->getColor(rvt::BLACK); 316 | 317 | // Visualize Results ------------------------------------------------------------------------------------------------- 318 | for (std::size_t marker_id = 0; marker_id < image->getSize(); ++marker_id) 319 | { 320 | unsigned int x = marker_id % image->x; // Map index back to coordinates 321 | unsigned int y = marker_id / image->x; // Map index back to coordinates 322 | 323 | std_msgs::ColorRGBA color; 324 | color.r = image->data[image->getID(x, y)].red / 255.0; 325 | color.g = image->data[image->getID(x, y)].green / 255.0; 326 | color.b = image->data[image->getID(x, y)].blue / 255.0; 327 | if (color.r + color.g + color.b > 3.0 - 2.0*std::numeric_limits::epsilon()) 328 | { 329 | continue; // transparent, do not publish 330 | } 331 | 332 | // Overide to black 333 | color = visuals_->getColor(rvt::DARK_GREY); 334 | color.a = 1.0; 335 | 336 | // Make right and down triangle 337 | // Check that we are not on the far right or bottom 338 | if (!(x + 1 >= image->x || y + 1 >= image->y)) 339 | { 340 | // Top left triangle 341 | publishTriangle(x, y, &marker, color); 342 | publishTriangle(x + 1, y, &marker, color); 343 | publishTriangle(x + 1, y + 1, &marker, color); 344 | 345 | // Bottom right triangle 346 | publishTriangle(x, y, &marker, color); 347 | publishTriangle(x, y + 1, &marker, color); 348 | publishTriangle(x + 1, y + 1, &marker, color); 349 | } 350 | } 351 | 352 | // Send to Rviz 353 | return visuals_->publishMarker(marker); 354 | } 355 | 356 | bool ROSVizWindow::publishTriangle(int x, int y, visualization_msgs::Marker* marker, std_msgs::ColorRGBA color) 357 | { 358 | // Point 359 | temp_point_.x = x; 360 | temp_point_.y = y; 361 | temp_point_.z = 0.1; // all costs become zero in flat world 362 | 363 | marker->points.push_back(temp_point_); 364 | marker->colors.push_back(color); 365 | 366 | return true; 367 | } 368 | 369 | bool ROSVizWindow::publishEdge(const ob::State* stateA, const ob::State* stateB, const std_msgs::ColorRGBA& color, 370 | const double radius) 371 | { 372 | return visuals_->publishCylinder(stateToPoint(stateA), stateToPoint(stateB), color, radius / 2.0); 373 | } 374 | 375 | bool ROSVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, double scale, 376 | const std::string& ns) 377 | { 378 | geometry_msgs::Vector3 scale_vector; 379 | scale_vector.x = scale; 380 | scale_vector.y = scale; 381 | scale_vector.z = scale; 382 | return publishSpheres(path, color, scale_vector, ns); 383 | } 384 | 385 | bool ROSVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, const rvt::scales scale, 386 | const std::string& ns) 387 | { 388 | return publishSpheres(path, color, visuals_->getScale(scale), ns); 389 | } 390 | 391 | bool ROSVizWindow::publishSpheres(const og::PathGeometric& path, const rvt::colors& color, 392 | const geometry_msgs::Vector3& scale, const std::string& ns) 393 | { 394 | std::vector points; 395 | for (std::size_t i = 0; i < path.getStateCount(); ++i) 396 | points.push_back(visuals_->convertPoint(stateToPoint(path.getState(i)))); 397 | 398 | return visuals_->publishSpheres(points, color, scale, ns); 399 | } 400 | 401 | // Deprecated 402 | bool ROSVizWindow::publishPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness, 403 | const std::string& ns) 404 | { 405 | return publish2DPath(path, color, thickness, ns); 406 | } 407 | 408 | bool ROSVizWindow::publish2DPath(const og::PathGeometric& path, const rvt::colors& color, const double thickness, 409 | const std::string& ns) 410 | { 411 | // Error check 412 | if (path.getStateCount() <= 0) 413 | { 414 | ROS_WARN_STREAM_NAMED(name_, "No states found in path"); 415 | return false; 416 | } 417 | 418 | // Initialize first vertex 419 | Eigen::Vector3d prev_vertex = stateToPoint(path.getState(0)); 420 | Eigen::Vector3d this_vertex; 421 | 422 | // Convert path coordinates 423 | for (std::size_t i = 1; i < path.getStateCount(); ++i) 424 | { 425 | // Get current coordinates 426 | this_vertex = stateToPoint(path.getState(i)); 427 | 428 | // Create line 429 | visuals_->publishCylinder(prev_vertex, this_vertex, color, thickness, ns); 430 | 431 | // Save these coordinates for next line 432 | prev_vertex = this_vertex; 433 | } 434 | 435 | return true; 436 | } 437 | 438 | Eigen::Vector3d ROSVizWindow::stateToPoint(const ob::ScopedState<> state) 439 | { 440 | return stateToPoint(state.get()); 441 | } 442 | 443 | Eigen::Vector3d ROSVizWindow::stateToPoint(const ob::State* state) 444 | { 445 | if (!state) 446 | { 447 | ROS_FATAL_NAMED(name_, "No state found for vertex"); 448 | exit(1); 449 | } 450 | 451 | // Convert to RealVectorStateSpace 452 | const ob::RealVectorStateSpace::StateType* real_state = 453 | static_cast(state); 454 | 455 | // Create point 456 | temp_eigen_point_.x() = real_state->values[0]; 457 | temp_eigen_point_.y() = real_state->values[1]; 458 | 459 | if (si_->getStateSpace()->getDimension() == 2) 460 | temp_eigen_point_.z() = level_scale_ * si_->getStateSpace()->getLevel(state); 461 | else 462 | temp_eigen_point_.z() = real_state->values[2]; 463 | 464 | return temp_eigen_point_; 465 | } 466 | 467 | int ROSVizWindow::natRound(double x) 468 | { 469 | return static_cast(floor(x + 0.5f)); 470 | } 471 | 472 | bool ROSVizWindow::publishState(const ob::State* state, const rvt::colors& color, const rvt::scales scale, 473 | const std::string& ns) 474 | { 475 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 476 | } 477 | 478 | bool ROSVizWindow::publishState(const ob::State* state, const rvt::colors& color, const double scale, 479 | const std::string& ns) 480 | { 481 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 482 | } 483 | 484 | bool ROSVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, const rvt::scales scale, 485 | const std::string& ns) 486 | { 487 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 488 | } 489 | 490 | bool ROSVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, double scale, 491 | const std::string& ns) 492 | { 493 | return visuals_->publishSphere(stateToPoint(state), color, scale, ns); 494 | } 495 | 496 | bool ROSVizWindow::publishState(const ob::ScopedState<> state, const rvt::colors& color, 497 | const geometry_msgs::Vector3& scale, const std::string& ns) 498 | { 499 | return visuals_->publishSphere(stateToPoint(state), color, scale.x, ns); 500 | } 501 | 502 | bool ROSVizWindow::publishSampleRegion(const ob::ScopedState<>& state_area, const double& distance) 503 | { 504 | temp_point_.x = state_area[0]; 505 | temp_point_.y = state_area[1]; 506 | temp_point_.z = state_area[2]; 507 | 508 | visuals_->publishSphere(temp_point_, rvt::BLACK, rvt::REGULAR, "sample_region"); // mid point 509 | // outer sphere (x2 b/c its a radius, x0.1 to make it look nicer) 510 | return visuals_->publishSphere(temp_point_, rvt::TRANSLUCENT, rvt::REGULAR, "sample_region"); 511 | } 512 | 513 | } // namespace ompl_visual_tools 514 | -------------------------------------------------------------------------------- /src/utilities/ppm.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015, University of Colorado, Boulder 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Univ of CO, Boulder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* 36 | * PPM Image Format Reader 37 | * Code adapted from http://stackoverflow.com/questions/2693631/read-ppm-file-and-store-it-in-an-array-coded-with-c 38 | * User: rpf 39 | * 8/1/2012 40 | */ 41 | 42 | #include 43 | 44 | namespace ompl_visual_tools 45 | { 46 | 47 | // ************************************************************************************************* 48 | // Reading Function 49 | // ************************************************************************************************* 50 | PPMImage *readPPM(const char *filename) 51 | { 52 | char buff[16]; 53 | PPMImage *img; 54 | FILE *fp; 55 | int c, rgb_comp_color; 56 | 57 | //open PPM file for reading 58 | fp = fopen(filename, "rb"); 59 | if (!fp) { 60 | fprintf(stderr, "Unable to open file '%s'\n", filename); 61 | exit(1); 62 | } 63 | 64 | //read image format 65 | if (!fgets(buff, sizeof(buff), fp)) { 66 | perror(filename); 67 | exit(1); 68 | } 69 | 70 | //check the image format 71 | if (buff[0] != 'P' || buff[1] != '6') { 72 | fprintf(stderr, "Invalid image format (must be 'P6')\n"); 73 | exit(1); 74 | } 75 | 76 | //alloc memory form image 77 | img = (PPMImage *)malloc(sizeof(PPMImage)); 78 | if (!img) { 79 | fprintf(stderr, "Unable to allocate memory\n"); 80 | exit(1); 81 | } 82 | 83 | //check for comments 84 | c = getc(fp); 85 | while (c == '#') { 86 | while (getc(fp) != '\n') ; 87 | c = getc(fp); 88 | } 89 | 90 | ungetc(c, fp); 91 | //read image size information 92 | if (fscanf(fp, "%d %d", &img->x, &img->y) != 2) { 93 | fprintf(stderr, "Invalid image size (error loading '%s')\n", filename); 94 | exit(1); 95 | } 96 | 97 | //read rgb component 98 | if (fscanf(fp, "%d", &rgb_comp_color) != 1) { 99 | fprintf(stderr, "Invalid rgb component (error loading '%s')\n", filename); 100 | exit(1); 101 | } 102 | 103 | //check rgb component depth 104 | if (rgb_comp_color!= RGB_COMPONENT_COLOR) { 105 | fprintf(stderr, "'%s' does not have 8-bits components\n", filename); 106 | exit(1); 107 | } 108 | 109 | while (fgetc(fp) != '\n') ; 110 | //memory allocation for pixel data 111 | img->data = (PPMPixel*)malloc(img->x * img->y * sizeof(PPMPixel)); 112 | 113 | if (!img) { 114 | fprintf(stderr, "Unable to allocate memory\n"); 115 | exit(1); 116 | } 117 | 118 | //read pixel data from file 119 | if ( fread( img->data, 3 * img->x, img->y, fp) != img->y) { 120 | fprintf(stderr, "Error loading image '%s'\n", filename); 121 | exit(1); 122 | } 123 | 124 | // Close the file pointer 125 | fclose(fp); 126 | 127 | return img; 128 | } 129 | 130 | } // namespace 131 | --------------------------------------------------------------------------------