├── rviz_plugins ├── Makefile ├── lib │ └── librviz_plugins.so ├── src │ ├── goal_tool.h │ ├── pose_tool.h │ ├── goal_tool.cpp │ ├── multi_probmap_display.h │ ├── probmap_display.h │ ├── aerialmap_display.h │ ├── pose_tool.cpp │ ├── multi_probmap_display.cpp │ ├── aerialmap_display.cpp │ └── probmap_display.cpp ├── package.xml ├── plugin_description.xml └── CMakeLists.txt ├── waypoint_generator ├── package.xml ├── CMakeLists.txt └── src │ ├── sample_waypoints.h │ └── waypoint_generator.cpp ├── README.md ├── grid_path_searcher ├── package.xml ├── include │ ├── node.h │ ├── JPS_searcher.h │ ├── JPS_utils.h │ └── Astar_searcher.h ├── CMakeLists.txt ├── launch │ ├── demo.launch │ └── rviz_config │ │ └── demo.rviz └── src │ ├── random_complex_generator.cpp │ ├── demo_node.cpp │ ├── JPS_searcher.cpp │ ├── Astar_searcher.cpp │ └── JPS_utils.cpp └── LICENSE /rviz_plugins/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /rviz_plugins/lib/librviz_plugins.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Grizi-ju/Astar_JPS_Pathplanning_in_ROS/HEAD/rviz_plugins/lib/librviz_plugins.so -------------------------------------------------------------------------------- /waypoint_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 0.0.0 3 | waypoint_generator 4 | 5 | 6 | waypoint_generator 7 | 8 | 9 | Shaojie Shen 10 | BSD 11 | http://ros.org/wiki/waypoint_generator 12 | 13 | catkin 14 | 15 | roscpp 16 | tf 17 | nav_msgs 18 | 19 | roscpp 20 | tf 21 | nav_msgs 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Astar_JPS_Pathplanning_in_ROS 2 | 智能车航天物流线上赛--在ROS中实现A星三维路径规划 3 | 4 | **使用：** 5 | 6 | 点个Star 谢谢~ 7 | 8 | Ubuntu18.04 9 | melodic 10 | 11 | `````` 12 | 1.git clone git@github.com:Grizi-ju/Astar_JPS_Pathplanning_in_ROS.git 13 | 14 | 2.cd catkin_ws 15 | 16 | 3.catkin_make 17 | 18 | 4.source devel/setup.bash 19 | 20 | 5.roslaunch grid_path_searcher demo.launch 21 | `````` 22 | 23 | 24 | 实际还有很多改进的地方，时间原因暂时是这样，后续会继续提交 25 | 26 | **结果：** 27 | 28 | ![image](https://user-images.githubusercontent.com/80267952/162573084-6a798240-ffc8-4776-87f5-a922429c6637.png) 29 | 30 | 31 | Reference: 32 | 33 | https://blog.csdn.net/weixin_48628189/article/details/119545751?spm=1001.2014.3001.5502 34 | 35 | https://github.com/Mesywang/Astar-JPS-Algorithm 36 | 37 | https://mesywang.github.io/2020/01/26/Jump-Point-Search/ 38 | 39 | 《Improving Jump Point Search》 40 | -------------------------------------------------------------------------------- /grid_path_searcher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grid_path_searcher 4 | 0.0.1 5 | Path searching methods on 3d grid map 6 | fgaoaa 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | visualization_msgs 16 | 17 | roscpp 18 | rospy 19 | sensor_msgs 20 | std_msgs 21 | visualization_msgs 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /rviz_plugins/src/goal_tool.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef RVIZ_GOAL_TOOL_H 3 | #define RVIZ_GOAL_TOOL_H 4 | 5 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 6 | # include 7 | 8 | # include 9 | 10 | # include "pose_tool.h" 11 | #endif 12 | 13 | namespace rviz 14 | { 15 | class Arrow; 16 | class DisplayContext; 17 | class StringProperty; 18 | 19 | class Goal3DTool: public Pose3DTool 20 | { 21 | Q_OBJECT 22 | public: 23 | Goal3DTool(); 24 | virtual ~Goal3DTool() {} 25 | virtual void onInitialize(); 26 | 27 | protected: 28 | virtual void onPoseSet(double x, double y, double z, double theta); 29 | 30 | private Q_SLOTS: 31 | void updateTopic(); 32 | 33 | private: 34 | ros::NodeHandle nh_; 35 | ros::Publisher pub_; 36 | 37 | StringProperty* topic_property_; 38 | }; 39 | 40 | } 41 | 42 | #endif 43 | 44 | 45 | -------------------------------------------------------------------------------- /rviz_plugins/src/pose_tool.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef RVIZ_POSE_TOOL_H 3 | #define RVIZ_POSE_TOOL_H 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include "rviz/tool.h" 12 | 13 | namespace rviz 14 | { 15 | class Arrow; 16 | class DisplayContext; 17 | 18 | class Pose3DTool: public Tool 19 | { 20 | public: 21 | Pose3DTool(); 22 | virtual ~Pose3DTool(); 23 | 24 | virtual void onInitialize(); 25 | 26 | virtual void activate(); 27 | virtual void deactivate(); 28 | 29 | virtual int processMouseEvent( ViewportMouseEvent& event ); 30 | 31 | protected: 32 | virtual void onPoseSet(double x, double y, double z, double theta) = 0; 33 | 34 | Arrow* arrow_; 35 | std::vector arrow_array; 36 | 37 | enum State 38 | { 39 | Position, 40 | Orientation, 41 | Height 42 | }; 43 | State state_; 44 | 45 | Ogre::Vector3 pos_; 46 | }; 47 | 48 | } 49 | 50 | #endif 51 | 52 | 53 | -------------------------------------------------------------------------------- /grid_path_searcher/include/node.h: -------------------------------------------------------------------------------- 1 | #ifndef _NODE_H_ 2 | #define _NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "backward.hpp" 9 | 10 | #define inf 1>>20 11 | struct GridNode; 12 | 13 | typedef GridNode* GridNodePtr; 14 | 15 | 16 | struct GridNode 17 | { 18 | int id; 19 | 20 | Eigen::Vector3d coord; 21 | 22 | Eigen::Vector3i dir; 23 | 24 | Eigen::Vector3i index; 25 | 26 | double gScore, fScore; 27 | 28 | GridNodePtr cameFrom; 29 | std::multimap::iterator nodeMapIt; 30 | 31 | GridNode(Eigen::Vector3i _index, Eigen::Vector3d _coord){ 32 | id = 0; 33 | index = _index; 34 | coord = _coord; 35 | dir = Eigen::Vector3i::Zero(); 36 | 37 | gScore = inf; 38 | fScore = inf; 39 | 40 | cameFrom = NULL; 41 | } 42 | 43 | GridNode(){}; 44 | ~GridNode(){}; 45 | }; 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /rviz_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 0.0.0 3 | rviz_plugins 4 | 5 | 6 | Additional plugins for RViz 7 | 8 | 9 | Shaojie Shen 10 | BSD 11 | http://ros.org/wiki/rviz_plugins 12 | 13 | catkin 14 | 15 | rviz 16 | roscpp 17 | 18 | qtbase5-dev 19 | 20 | rviz 21 | 22 | roscpp 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rviz_plugins/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Tool for setting 3D goal 7 | 8 | 9 | 12 | 13 | Display of -inf +inf probabilistic occupancy grid map 14 | 15 | 16 | 19 | 20 | Display of aerial map 21 | 22 | 23 | 26 | 27 | Display of multiple -inf +inf probabilistic occupancy grid map 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /grid_path_searcher/include/JPS_searcher.h: -------------------------------------------------------------------------------- 1 | #ifndef _JPS_SEARCHER_H_ 2 | #define _JPS_SEARCHER_H_ 3 | 4 | #include "Astar_searcher.h" 5 | #include "JPS_utils.h" 6 | 7 | class JPSPathFinder : public AstarPathFinder 8 | { 9 | private: 10 | bool isOccupied(const int &idx_x, const int &idx_y, const int &idx_z) const; 11 | bool isOccupied(const Eigen::Vector3i &index) const; 12 | bool isFree(const int &idx_x, const int &idx_y, const int &idx_z) const; 13 | bool isFree(const Eigen::Vector3i &index) const; 14 | 15 | public: 16 | JPS3DNeib *jn3d; 17 | 18 | JPSPathFinder() 19 | { 20 | jn3d = new JPS3DNeib(); 21 | }; 22 | 23 | ~JPSPathFinder() 24 | { 25 | delete jn3d; 26 | }; 27 | void JPSGetSucc(GridNodePtr currentPtr, std::vector &neighborPtrSets, std::vector &edgeCostSets); 28 | bool hasForced(const Eigen::Vector3i &idx, const Eigen::Vector3i &dir); 29 | bool jump(const Eigen::Vector3i &curIdx, const Eigen::Vector3i &expDir, Eigen::Vector3i &neiIdx); 30 | 31 | void JPSGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt); 32 | }; 33 | 34 | #endif -------------------------------------------------------------------------------- /waypoint_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_generator) 3 | 4 | set(CMAKE_VERBOSE_MAKEFILE "true") 5 | include(CheckCXXCompilerFlag) 6 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 7 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 8 | if(COMPILER_SUPPORTS_CXX11) 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 10 | elseif(COMPILER_SUPPORTS_CXX0X) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 12 | else() 13 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 14 | endif() 15 | 16 | set(ADDITIONAL_CXX_FLAG "-Wall -O3 -march=native") 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}") 18 | 19 | find_package(catkin REQUIRED COMPONENTS 20 | roscpp 21 | tf 22 | nav_msgs 23 | ) 24 | catkin_package() 25 | 26 | include_directories(include) 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | 31 | add_executable(waypoint_generator src/waypoint_generator.cpp) 32 | 33 | target_link_libraries(waypoint_generator 34 | ${catkin_LIBRARIES} 35 | ) 36 | -------------------------------------------------------------------------------- /grid_path_searcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(grid_path_searcher) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | nav_msgs 8 | visualization_msgs 9 | ) 10 | 11 | find_package(Eigen3 REQUIRED) 12 | find_package(PCL REQUIRED) 13 | 14 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include 18 | ) 19 | 20 | include_directories( 21 | include 22 | SYSTEM 23 | third_party 24 | ${catkin_INCLUDE_DIRS} 25 | ${Eigen3_INCLUDE_DIRS} 26 | ${PCL_INCLUDE_DIRS} 27 | ) 28 | 29 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") # -Wextra -Werror 30 | 31 | add_executable( demo_node 32 | src/demo_node.cpp 33 | src/Astar_searcher.cpp 34 | src/JPS_utils.cpp 35 | src/JPS_searcher.cpp 36 | ) 37 | 38 | target_link_libraries(demo_node 39 | ${catkin_LIBRARIES} 40 | ${PCL_LIBRARIES} 41 | ) 42 | 43 | add_executable ( random_complex 44 | src/random_complex_generator.cpp ) 45 | 46 | target_link_libraries( random_complex 47 | ${catkin_LIBRARIES} 48 | ${PCL_LIBRARIES} ) 49 | -------------------------------------------------------------------------------- /grid_path_searcher/include/JPS_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _JPS_UTILS_H_ 2 | #define _JPS_UTILS_H_ 3 | 4 | #include 5 | ///Search and prune neighbors for JPS 3D 6 | struct JPS3DNeib 7 | { 8 | // for each (dx,dy,dz) these contain: 9 | // ns: neighbors that are always added 10 | // f1: forced neighbors to check 11 | // f2: neighbors to add if f1 is forced 12 | int ns[27][3][26]; 13 | int f1[27][3][12]; 14 | int f2[27][3][12]; 15 | // nsz contains the number of neighbors for the four different types of moves: 16 | // no move (norm 0): 26 neighbors always added 17 | // 0 forced neighbors to check (never happens) 18 | // 0 neighbors to add if forced (never happens) 19 | // straight (norm 1): 1 neighbor always added 20 | // 8 forced neighbors to check 21 | // 8 neighbors to add if forced 22 | // diagonal (norm sqrt(2)): 3 neighbors always added 23 | // 8 forced neighbors to check 24 | // 12 neighbors to add if forced 25 | // diagonal (norm sqrt(3)): 7 neighbors always added 26 | // 6 forced neighbors to check 27 | // 12 neighbors to add if forced 28 | static constexpr int nsz[4][2] = {{26, 0}, {1, 8}, {3, 12}, {7, 12}}; 29 | JPS3DNeib(); 30 | 31 | private: 32 | void Neib(int dx, int dy, int dz, int norm1, int dev, int &tx, int &ty, int &tz); 33 | void FNeib(int dx, int dy, int dz, int norm1, int dev, 34 | int &fx, int &fy, int &fz, 35 | int &nx, int &ny, int &nz); 36 | }; 37 | 38 | #endif -------------------------------------------------------------------------------- /rviz_plugins/src/goal_tool.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "rviz/display_context.h" 8 | #include "rviz/properties/string_property.h" 9 | 10 | #include "goal_tool.h" 11 | 12 | namespace rviz 13 | { 14 | 15 | Goal3DTool::Goal3DTool() 16 | { 17 | shortcut_key_ = 'g'; 18 | 19 | topic_property_ = new StringProperty( "Topic", "goal", 20 | "The topic on which to publish navigation goals.", 21 | getPropertyContainer(), SLOT( updateTopic() ), this ); 22 | } 23 | 24 | void Goal3DTool::onInitialize() 25 | { 26 | Pose3DTool::onInitialize(); 27 | setName( "3D Nav Goal" ); 28 | updateTopic(); 29 | } 30 | 31 | void Goal3DTool::updateTopic() 32 | { 33 | pub_ = nh_.advertise( topic_property_->getStdString(), 1 ); 34 | } 35 | 36 | void Goal3DTool::onPoseSet(double x, double y, double z, double theta) 37 | { 38 | ROS_WARN("3D Goal Set"); 39 | std::string fixed_frame = context_->getFixedFrame().toStdString(); 40 | tf::Quaternion quat; 41 | quat.setRPY(0.0, 0.0, theta); 42 | tf::Stamped p = tf::Stamped(tf::Pose(quat, tf::Point(x, y, z)), ros::Time::now(), fixed_frame); 43 | geometry_msgs::PoseStamped goal; 44 | tf::poseStampedTFToMsg(p, goal); 45 | ROS_INFO("Setting goal: Frame:%s, Position(%.3f, %.3f, %.3f), Orientation(%.3f, %.3f, %.3f, %.3f) = Angle: %.3f\n", fixed_frame.c_str(), 46 | goal.pose.position.x, goal.pose.position.y, goal.pose.position.z, 47 | goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w, theta); 48 | pub_.publish(goal); 49 | } 50 | 51 | } 52 | 53 | #include 54 | PLUGINLIB_EXPORT_CLASS( rviz::Goal3DTool, rviz::Tool ) 55 | -------------------------------------------------------------------------------- /rviz_plugins/src/multi_probmap_display.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef MULTI_PROB_MAP_DISPLAY_H 3 | #define MULTI_PROB_MAP_DISPLAY_H 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include "rviz/display.h" 16 | 17 | namespace Ogre 18 | { 19 | class ManualObject; 20 | } 21 | 22 | namespace rviz 23 | { 24 | 25 | class FloatProperty; 26 | class IntProperty; 27 | class Property; 28 | class QuaternionProperty; 29 | class RosTopicProperty; 30 | class VectorProperty; 31 | 32 | class MultiProbMapDisplay: public Display 33 | { 34 | Q_OBJECT 35 | public: 36 | MultiProbMapDisplay(); 37 | virtual ~MultiProbMapDisplay(); 38 | 39 | virtual void onInitialize(); 40 | virtual void reset(); 41 | virtual void update( float wall_dt, float ros_dt ); 42 | 43 | protected Q_SLOTS: 44 | void updateTopic(); 45 | void updateDrawUnder(); 46 | 47 | 48 | protected: 49 | 50 | virtual void onEnable(); 51 | virtual void onDisable(); 52 | 53 | virtual void subscribe(); 54 | virtual void unsubscribe(); 55 | 56 | void incomingMap(const multi_map_server::MultiOccupancyGrid::ConstPtr& msg); 57 | 58 | void clear(); 59 | 60 | std::vector manual_object_; 61 | std::vector texture_; 62 | std::vector material_; 63 | 64 | bool loaded_; 65 | 66 | std::string topic_; 67 | 68 | ros::Subscriber map_sub_; 69 | 70 | RosTopicProperty* topic_property_; 71 | Property* draw_under_property_; 72 | 73 | multi_map_server::MultiOccupancyGrid::ConstPtr updated_map_; 74 | multi_map_server::MultiOccupancyGrid::ConstPtr current_map_; 75 | boost::mutex mutex_; 76 | bool new_map_; 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /grid_path_searcher/include/Astar_searcher.h: -------------------------------------------------------------------------------- 1 | #ifndef _ASTART_SEARCHER_H 2 | #define _ASTART_SEARCHER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "backward.hpp" 9 | #include "node.h" 10 | 11 | class AstarPathFinder 12 | { 13 | private: 14 | 15 | protected: 16 | uint8_t * data; 17 | 18 | GridNodePtr *** GridNodeMap; 19 | 20 | Eigen::Vector3i goalIdx; 21 | 22 | int GLX_SIZE, GLY_SIZE, GLZ_SIZE; 23 | int GLXYZ_SIZE, GLYZ_SIZE; 24 | 25 | double resolution, inv_resolution; 26 | 27 | double gl_xl, gl_yl, gl_zl; 28 | double gl_xu, gl_yu, gl_zu; 29 | 30 | GridNodePtr terminatePtr; 31 | 32 | std::multimap openSet; 33 | 34 | double getHeu(GridNodePtr node1, GridNodePtr node2); 35 | double getHeu2(GridNodePtr node1, GridNodePtr node2); 36 | 37 | double calEuclideanDistance(GridNodePtr node1, GridNodePtr node2); 38 | 39 | void AstarGetSucc(GridNodePtr currentPtr, std::vector & neighborPtrSets, std::vector & edgeCostSets); 40 | 41 | bool isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const; 42 | bool isOccupied(const Eigen::Vector3i & index) const; 43 | 44 | bool isFree(const int & idx_x, const int & idx_y, const int & idx_z) const; 45 | bool isFree(const Eigen::Vector3i & index) const; 46 | 47 | Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index); 48 | Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt); 49 | 50 | public: 51 | AstarPathFinder(){}; 52 | ~AstarPathFinder(){}; 53 | 54 | void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt); 55 | 56 | void resetGrid(GridNodePtr ptr); 57 | 58 | void resetUsedGrids(); 59 | 60 | void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id); 61 | 62 | void setObs(const double coord_x, const double coord_y, const double coord_z); 63 | 64 | Eigen::Vector3d coordRounding(const Eigen::Vector3d & coord); 65 | 66 | std::vector getPath(); 67 | 68 | std::vector getVisitedNodes(); 69 | }; 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /rviz_plugins/src/probmap_display.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PROB_MAP_DISPLAY_H 3 | #define PROB_MAP_DISPLAY_H 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "rviz/display.h" 15 | 16 | namespace Ogre 17 | { 18 | class ManualObject; 19 | } 20 | 21 | namespace rviz 22 | { 23 | 24 | class FloatProperty; 25 | class IntProperty; 26 | class Property; 27 | class QuaternionProperty; 28 | class RosTopicProperty; 29 | class VectorProperty; 30 | 31 | class ProbMapDisplay: public Display 32 | { 33 | Q_OBJECT 34 | public: 35 | ProbMapDisplay(); 36 | virtual ~ProbMapDisplay(); 37 | 38 | virtual void onInitialize(); 39 | virtual void fixedFrameChanged(); 40 | virtual void reset(); 41 | virtual void update( float wall_dt, float ros_dt ); 42 | 43 | float getResolution() { return resolution_; } 44 | int getWidth() { return width_; } 45 | int getHeight() { return height_; } 46 | Ogre::Vector3 getPosition() { return position_; } 47 | Ogre::Quaternion getOrientation() { return orientation_; } 48 | 49 | protected Q_SLOTS: 50 | void updateAlpha(); 51 | void updateTopic(); 52 | void updateDrawUnder(); 53 | 54 | 55 | protected: 56 | 57 | virtual void onEnable(); 58 | virtual void onDisable(); 59 | 60 | virtual void subscribe(); 61 | virtual void unsubscribe(); 62 | 63 | void incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); 64 | 65 | void clear(); 66 | 67 | void transformMap(); 68 | 69 | Ogre::ManualObject* manual_object_; 70 | Ogre::TexturePtr texture_; 71 | Ogre::MaterialPtr material_; 72 | bool loaded_; 73 | 74 | std::string topic_; 75 | float resolution_; 76 | int width_; 77 | int height_; 78 | Ogre::Vector3 position_; 79 | Ogre::Quaternion orientation_; 80 | std::string frame_; 81 | 82 | ros::Subscriber map_sub_; 83 | 84 | RosTopicProperty* topic_property_; 85 | FloatProperty* resolution_property_; 86 | IntProperty* width_property_; 87 | IntProperty* height_property_; 88 | VectorProperty* position_property_; 89 | QuaternionProperty* orientation_property_; 90 | FloatProperty* alpha_property_; 91 | Property* draw_under_property_; 92 | 93 | nav_msgs::OccupancyGrid::ConstPtr updated_map_; 94 | nav_msgs::OccupancyGrid::ConstPtr current_map_; 95 | boost::mutex mutex_; 96 | bool new_map_; 97 | }; 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /rviz_plugins/src/aerialmap_display.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef AERIAL_MAP_DISPLAY_H 3 | #define AERIAL_MAP_DISPLAY_H 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "rviz/display.h" 15 | 16 | namespace Ogre 17 | { 18 | class ManualObject; 19 | } 20 | 21 | namespace rviz 22 | { 23 | 24 | class FloatProperty; 25 | class IntProperty; 26 | class Property; 27 | class QuaternionProperty; 28 | class RosTopicProperty; 29 | class VectorProperty; 30 | 31 | class AerialMapDisplay: public Display 32 | { 33 | Q_OBJECT 34 | public: 35 | AerialMapDisplay(); 36 | virtual ~AerialMapDisplay(); 37 | 38 | virtual void onInitialize(); 39 | virtual void fixedFrameChanged(); 40 | virtual void reset(); 41 | virtual void update( float wall_dt, float ros_dt ); 42 | 43 | float getResolution() { return resolution_; } 44 | int getWidth() { return width_; } 45 | int getHeight() { return height_; } 46 | Ogre::Vector3 getPosition() { return position_; } 47 | Ogre::Quaternion getOrientation() { return orientation_; } 48 | 49 | protected Q_SLOTS: 50 | void updateAlpha(); 51 | void updateTopic(); 52 | void updateDrawUnder(); 53 | 54 | 55 | protected: 56 | 57 | virtual void onEnable(); 58 | virtual void onDisable(); 59 | 60 | virtual void subscribe(); 61 | virtual void unsubscribe(); 62 | 63 | void incomingAerialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg); 64 | 65 | void clear(); 66 | 67 | void transformAerialMap(); 68 | 69 | Ogre::ManualObject* manual_object_; 70 | Ogre::TexturePtr texture_; 71 | Ogre::MaterialPtr material_; 72 | bool loaded_; 73 | 74 | std::string topic_; 75 | float resolution_; 76 | int width_; 77 | int height_; 78 | Ogre::Vector3 position_; 79 | Ogre::Quaternion orientation_; 80 | std::string frame_; 81 | 82 | ros::Subscriber map_sub_; 83 | 84 | RosTopicProperty* topic_property_; 85 | FloatProperty* resolution_property_; 86 | IntProperty* width_property_; 87 | IntProperty* height_property_; 88 | VectorProperty* position_property_; 89 | QuaternionProperty* orientation_property_; 90 | FloatProperty* alpha_property_; 91 | Property* draw_under_property_; 92 | 93 | nav_msgs::OccupancyGrid::ConstPtr updated_map_; 94 | nav_msgs::OccupancyGrid::ConstPtr current_map_; 95 | boost::mutex mutex_; 96 | bool new_map_; 97 | }; 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /grid_path_searcher/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /rviz_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_plugins) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rviz 9 | #multi_map_server 10 | roscpp 11 | ) 12 | 13 | ################################### 14 | ## catkin specific configuration ## 15 | ################################### 16 | ## The catkin_package macro generates cmake config files for your package 17 | ## Declare things to be passed to dependent projects 18 | ## INCLUDE_DIRS: uncomment this if you package contains header files 19 | ## LIBRARIES: libraries you create in this project that dependent projects also need 20 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 21 | ## DEPENDS: system dependencies of this project that dependent projects also need 22 | catkin_package( 23 | # INCLUDE_DIRS include 24 | # LIBRARIES irobot_msgs 25 | CATKIN_DEPENDS #multi_map_server 26 | # DEPENDS system_lib 27 | ) 28 | 29 | ########### 30 | ## Build ## 31 | ########### 32 | 33 | #find_package(Armadillo REQUIRED) 34 | #include_directories(${ARMADILLO_INCLUDE_DIRS}) 35 | 36 | ## This plugin includes Qt widgets, so we must include Qt like so: 37 | #find_package(Qt5 COMPONENTS QtCore QtGui REQUIRED) 38 | 39 | if(rviz_QT_VERSION VERSION_LESS "5") 40 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 41 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 42 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 43 | include(${QT_USE_FILE}) 44 | else() 45 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 46 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 47 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 48 | set(QT_LIBRARIES Qt5::Widgets) 49 | endif() 50 | 51 | ## I prefer the Qt signals and slots to avoid defining "emit", "slots", 52 | ## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. 53 | add_definitions(-DQT_NO_KEYWORDS) 54 | 55 | ## Here we specify which header files need to be run through "moc", 56 | ## Qt's meta-object compiler. 57 | qt5_wrap_cpp(MOC_FILES 58 | src/goal_tool.h 59 | # src/probmap_display.h 60 | # src/aerialmap_display.h 61 | # src/multi_probmap_display.h 62 | ) 63 | 64 | ## Here we specify the list of source files, including the output of 65 | ## the previous command which is stored in ``${MOC_FILES}``. 66 | set(SOURCE_FILES 67 | src/pose_tool.cpp 68 | src/goal_tool.cpp 69 | # src/probmap_display.cpp 70 | # src/aerialmap_display.cpp 71 | # src/multi_probmap_display.cpp 72 | ${MOC_FILES} 73 | ) 74 | 75 | 76 | ## Specify additional locations of header files 77 | ## Your package locations should be listed before other locations 78 | include_directories(include) 79 | include_directories( 80 | ${catkin_INCLUDE_DIRS} 81 | ) 82 | 83 | add_library(${PROJECT_NAME} ${SOURCE_FILES}) 84 | 85 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES}) 86 | 87 | install(TARGETS 88 | ${PROJECT_NAME} 89 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 90 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 91 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 92 | ) 93 | 94 | install(FILES 95 | plugin_description.xml 96 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 97 | 98 | install(DIRECTORY media/ 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media) 100 | 101 | install(DIRECTORY icons/ 102 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 103 | -------------------------------------------------------------------------------- /rviz_plugins/src/pose_tool.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "rviz/geometry.h" 8 | #include "rviz/ogre_helpers/arrow.h" 9 | #include "rviz/viewport_mouse_event.h" 10 | #include "rviz/load_resource.h" 11 | #include "rviz/render_panel.h" 12 | 13 | #include "pose_tool.h" 14 | 15 | namespace rviz 16 | { 17 | 18 | Pose3DTool::Pose3DTool() 19 | : Tool() 20 | , arrow_( NULL ) 21 | { 22 | } 23 | 24 | Pose3DTool::~Pose3DTool() 25 | { 26 | delete arrow_; 27 | } 28 | 29 | void Pose3DTool::onInitialize() 30 | { 31 | arrow_ = new Arrow( scene_manager_, NULL, 2.0f, 0.2f, 0.5f, 0.35f ); 32 | arrow_->setColor( 0.0f, 1.0f, 0.0f, 1.0f ); 33 | arrow_->getSceneNode()->setVisible( false ); 34 | } 35 | 36 | void Pose3DTool::activate() 37 | { 38 | setStatus( "Click and drag mouse to set position/orientation." ); 39 | state_ = Position; 40 | } 41 | 42 | void Pose3DTool::deactivate() 43 | { 44 | arrow_->getSceneNode()->setVisible( false ); 45 | } 46 | 47 | int Pose3DTool::processMouseEvent( ViewportMouseEvent& event ) 48 | { 49 | int flags = 0; 50 | static Ogre::Vector3 ang_pos; 51 | static double initz; 52 | static double prevz; 53 | static double prevangle; 54 | const double z_scale = 50; 55 | const double z_interval = 0.5; 56 | Ogre::Quaternion orient_x = Ogre::Quaternion( Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y ); 57 | 58 | if( event.leftDown() ) 59 | { 60 | ROS_ASSERT( state_ == Position ); 61 | Ogre::Vector3 intersection; 62 | Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f ); 63 | if( getPointOnPlaneFromWindowXY( event.viewport, 64 | ground_plane, 65 | event.x, event.y, intersection )) 66 | { 67 | pos_ = intersection; 68 | arrow_->setPosition( pos_ ); 69 | state_ = Orientation; 70 | flags |= Render; 71 | } 72 | } 73 | else if( event.type == QEvent::MouseMove && event.left() ) 74 | { 75 | if( state_ == Orientation ) 76 | { 77 | 78 | Ogre::Vector3 cur_pos; 79 | Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0.0f ); 80 | if( getPointOnPlaneFromWindowXY( event.viewport, 81 | ground_plane, 82 | event.x, event.y, cur_pos )) 83 | { 84 | double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x ); 85 | arrow_->getSceneNode()->setVisible( true ); 86 | arrow_->setOrientation( Ogre::Quaternion( Ogre::Radian(angle), Ogre::Vector3::UNIT_Z ) * orient_x ); 87 | if ( event.right() ) 88 | state_ = Height; 89 | initz = pos_.z; 90 | prevz = event.y; 91 | prevangle = angle; 92 | flags |= Render; 93 | } 94 | } 95 | if ( state_ == Height ) 96 | { 97 | double z = event.y; 98 | double dz = z - prevz; 99 | prevz = z; 100 | pos_.z -= dz / z_scale; 101 | arrow_->setPosition( pos_ ); 102 | 103 | for (int k = 0; k < arrow_array.size(); k++) 104 | delete arrow_array[k]; 105 | arrow_array.clear(); 106 | int cnt = ceil( fabs(initz - pos_.z) / z_interval ); 107 | for (int k = 0; k < cnt; k++) 108 | { 109 | Arrow* arrow__; 110 | arrow__ = new Arrow( scene_manager_, NULL, 0.5f, 0.1f, 0.0f, 0.1f ); 111 | arrow__->setColor( 0.0f, 1.0f, 0.0f, 1.0f ); 112 | arrow__->getSceneNode()->setVisible( true ); 113 | Ogre::Vector3 arr_pos = pos_; 114 | arr_pos.z = initz - ( (initz - pos_.z > 0)?1:-1 ) * k * z_interval; 115 | arrow__->setPosition( arr_pos ); 116 | arrow__->setOrientation( Ogre::Quaternion( Ogre::Radian(prevangle), Ogre::Vector3::UNIT_Z ) * orient_x ); 117 | arrow_array.push_back(arrow__); 118 | } 119 | flags |= Render; 120 | } 121 | } 122 | else if( event.leftUp() ) 123 | { 124 | if( state_ == Orientation || state_ == Height) 125 | { 126 | 127 | for (int k = 0; k < arrow_array.size(); k++) 128 | delete arrow_array[k]; 129 | arrow_array.clear(); 130 | onPoseSet(pos_.x, pos_.y, pos_.z, prevangle); 131 | flags |= (Finished|Render); 132 | } 133 | } 134 | 135 | return flags; 136 | } 137 | 138 | } 139 | 140 | -------------------------------------------------------------------------------- /waypoint_generator/src/sample_waypoints.h: -------------------------------------------------------------------------------- 1 | #ifndef SAMPLE_WAYPOINTS_H 2 | #define SAMPLE_WAYPOINTS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | nav_msgs::Path point() 9 | { 10 | 11 | nav_msgs::Path waypoints; 12 | geometry_msgs::PoseStamped pt; 13 | pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 14 | 15 | double h = 1.0; 16 | double scale = 7.0; 17 | 18 | pt.pose.position.y = scale * 0.0; 19 | pt.pose.position.x = scale * 2.0; 20 | pt.pose.position.z = h; 21 | waypoints.poses.push_back(pt); 22 | 23 | pt.pose.position.y = scale * 0.0; 24 | pt.pose.position.x = scale * 4.0; 25 | pt.pose.position.z = h; 26 | waypoints.poses.push_back(pt); 27 | 28 | pt.pose.position.y = scale * 0.25; 29 | pt.pose.position.x = scale * 5.0; 30 | pt.pose.position.z = h; 31 | waypoints.poses.push_back(pt); 32 | 33 | pt.pose.position.y = scale * 0.5; 34 | pt.pose.position.x = scale * 5.3; 35 | pt.pose.position.z = h; 36 | waypoints.poses.push_back(pt); 37 | 38 | pt.pose.position.y = scale * 0.75; 39 | pt.pose.position.x = scale * 5.0; 40 | pt.pose.position.z = h; 41 | waypoints.poses.push_back(pt); 42 | 43 | pt.pose.position.y = scale * 1.0; 44 | pt.pose.position.x = scale * 4.0; 45 | pt.pose.position.z = h; 46 | waypoints.poses.push_back(pt); 47 | 48 | pt.pose.position.y = scale * 1.0; 49 | pt.pose.position.x = scale * 2.0; 50 | pt.pose.position.z = h; 51 | waypoints.poses.push_back(pt); 52 | 53 | pt.pose.position.y = scale * 1.0; 54 | pt.pose.position.x = scale * 0.0; 55 | pt.pose.position.z = h; 56 | waypoints.poses.push_back(pt); 57 | 58 | return waypoints; 59 | } 60 | 61 | 62 | nav_msgs::Path circle() 63 | { 64 | double h = 1.0; 65 | double scale = 5.0; 66 | nav_msgs::Path waypoints; 67 | geometry_msgs::PoseStamped pt; 68 | pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 69 | 70 | pt.pose.position.y = -1.2 * scale; 71 | pt.pose.position.x = 2.5 * scale; 72 | pt.pose.position.z = h; 73 | waypoints.poses.push_back(pt); 74 | 75 | pt.pose.position.y = -2.4 * scale; 76 | pt.pose.position.x = 5.0 * scale; 77 | pt.pose.position.z = h; 78 | waypoints.poses.push_back(pt); 79 | pt.pose.position.y = 0.0 * scale; 80 | pt.pose.position.x = 5.0 * scale; 81 | pt.pose.position.z = h; 82 | waypoints.poses.push_back(pt); 83 | 84 | pt.pose.position.y = -1.2 * scale; 85 | pt.pose.position.x = 2.5 * scale; 86 | pt.pose.position.z = h; 87 | waypoints.poses.push_back(pt); 88 | 89 | pt.pose.position.y = -2.4 * scale; 90 | pt.pose.position.x = 0. * scale; 91 | pt.pose.position.z = h; 92 | waypoints.poses.push_back(pt); 93 | pt.pose.position.y = 0.0 * scale; 94 | pt.pose.position.x = 0.0 * scale; 95 | pt.pose.position.z = h; 96 | waypoints.poses.push_back(pt); 97 | 98 | pt.pose.position.y = -1.2 * scale; 99 | pt.pose.position.x = 2.5 * scale; 100 | pt.pose.position.z = h; 101 | waypoints.poses.push_back(pt); 102 | 103 | pt.pose.position.y = -2.4 * scale; 104 | pt.pose.position.x = 5.0 * scale; 105 | pt.pose.position.z = h; 106 | waypoints.poses.push_back(pt); 107 | pt.pose.position.y = 0.0 * scale; 108 | pt.pose.position.x = 5.0 * scale; 109 | pt.pose.position.z = h; 110 | waypoints.poses.push_back(pt); 111 | 112 | pt.pose.position.y = -1.2 * scale; 113 | pt.pose.position.x = 2.5 * scale; 114 | pt.pose.position.z = h; 115 | waypoints.poses.push_back(pt); 116 | 117 | pt.pose.position.y = -2.4 * scale; 118 | pt.pose.position.x = 0. * scale; 119 | pt.pose.position.z = h; 120 | waypoints.poses.push_back(pt); 121 | pt.pose.position.y = 0.0 * scale; 122 | pt.pose.position.x = 0.0 * scale; 123 | pt.pose.position.z = h; 124 | waypoints.poses.push_back(pt); 125 | 126 | 127 | return waypoints; 128 | } 129 | 130 | 131 | nav_msgs::Path eight() 132 | { 133 | 134 | double offset_x = 0.0; 135 | double offset_y = 0.0; 136 | double r = 10.0; 137 | double h = 2.0; 138 | nav_msgs::Path waypoints; 139 | geometry_msgs::PoseStamped pt; 140 | pt.pose.orientation = tf::createQuaternionMsgFromYaw(0.0); 141 | 142 | for(int i=0; i< 1; ++i) 143 | { 144 | 145 | pt.pose.position.x = r + offset_x; 146 | pt.pose.position.y = -r + offset_y; 147 | pt.pose.position.z = h/2; 148 | waypoints.poses.push_back(pt); 149 | pt.pose.position.x = r*2 + offset_x * 2; 150 | pt.pose.position.y = 0 ; 151 | pt.pose.position.z = h; 152 | waypoints.poses.push_back(pt); 153 | pt.pose.position.x = r*3 + offset_x * 3; 154 | pt.pose.position.y = r ; 155 | pt.pose.position.z = h/2; 156 | waypoints.poses.push_back(pt); 157 | pt.pose.position.x = r*4 + offset_x * 4; 158 | pt.pose.position.y = 0 ; 159 | pt.pose.position.z = h; 160 | waypoints.poses.push_back(pt); 161 | pt.pose.position.x = r*3 + offset_x * 3; 162 | pt.pose.position.y = -r ; 163 | pt.pose.position.z = h/2; 164 | waypoints.poses.push_back(pt); 165 | pt.pose.position.x = r*2 + offset_x * 2; 166 | pt.pose.position.y = 0 ; 167 | pt.pose.position.z = h; 168 | waypoints.poses.push_back(pt); 169 | pt.pose.position.x = r + offset_x * 2; 170 | pt.pose.position.y = r ; 171 | pt.pose.position.z = h/2; 172 | waypoints.poses.push_back(pt); 173 | pt.pose.position.x = 0 + offset_x; 174 | pt.pose.position.y = 0; 175 | pt.pose.position.z = h; 176 | waypoints.poses.push_back(pt); 177 | 178 | pt.pose.position.x = r + offset_x; 179 | pt.pose.position.y = -r; 180 | pt.pose.position.z = h / 2 * 3; 181 | waypoints.poses.push_back(pt); 182 | pt.pose.position.x = r*2 + offset_x * 2; 183 | pt.pose.position.y = 0; 184 | pt.pose.position.z = h; 185 | waypoints.poses.push_back(pt); 186 | pt.pose.position.x = r*3 + offset_x * 3; 187 | pt.pose.position.y = r; 188 | pt.pose.position.z = h / 2 * 3; 189 | waypoints.poses.push_back(pt); 190 | pt.pose.position.x = r*4 + offset_x * 4; 191 | pt.pose.position.y = 0; 192 | pt.pose.position.z = h; 193 | waypoints.poses.push_back(pt); 194 | pt.pose.position.x = r*3 + offset_x * 3; 195 | pt.pose.position.y = -r; 196 | pt.pose.position.z = h / 2 * 3; 197 | waypoints.poses.push_back(pt); 198 | pt.pose.position.x = r*2 + offset_x * 2; 199 | pt.pose.position.y = 0; 200 | pt.pose.position.z = h; 201 | waypoints.poses.push_back(pt); 202 | pt.pose.position.x = r + offset_x; 203 | pt.pose.position.y = r + offset_y; 204 | pt.pose.position.z = h / 2 * 3; 205 | waypoints.poses.push_back(pt); 206 | pt.pose.position.x = 0; 207 | pt.pose.position.y = 0; 208 | pt.pose.position.z = h; 209 | waypoints.poses.push_back(pt); 210 | } 211 | return waypoints; 212 | } 213 | #endif 214 | -------------------------------------------------------------------------------- /grid_path_searcher/launch/rviz_config/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /AllMap1/Autocompute Value Bounds1 8 | - /visitedNodes1/Namespaces1 9 | - /gridPath1/Namespaces1 10 | Splitter Ratio: 0.609022558 11 | Tree Height: 566 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: ~ 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: AllMap 28 | Toolbars: 29 | toolButtonStyle: 2 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 0.200000003 35 | Class: rviz/Grid 36 | Color: 0; 0; 0 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 50 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/Axes 52 | Enabled: true 53 | Length: 2 54 | Name: Axes 55 | Radius: 0.0500000007 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 1.89999998 62 | Min Value: 0.100000001 63 | Value: true 64 | Axis: Z 65 | Channel Name: intensity 66 | Class: rviz/PointCloud2 67 | Color: 193; 193; 193 68 | Color Transformer: AxisColor 69 | Decay Time: 0 70 | Enabled: true 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Max Intensity: 4096 74 | Min Color: 0; 0; 0 75 | Min Intensity: 0 76 | Name: AllMap 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.200000003 82 | Style: Boxes 83 | Topic: /demo_node/grid_map_vis 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: true 88 | - Class: rviz/Marker 89 | Enabled: false 90 | Marker Topic: /demo_node/visited_nodes_vis 91 | Name: visitedNodes 92 | Namespaces: 93 | {} 94 | Queue Size: 100 95 | Value: false 96 | - Class: rviz/Marker 97 | Enabled: true 98 | Marker Topic: /demo_node/grid_path_vis 99 | Name: gridPath 100 | Namespaces: 101 | demo_node/jps_path: true 102 | Queue Size: 100 103 | Value: true 104 | Enabled: true 105 | Global Options: 106 | Background Color: 255; 255; 127 107 | Default Light: true 108 | Fixed Frame: world 109 | Frame Rate: 30 110 | Name: root 111 | Tools: 112 | - Class: rviz/Interact 113 | Hide Inactive Objects: true 114 | - Class: rviz/MoveCamera 115 | - Class: rviz/FocusCamera 116 | - Class: rviz_plugins/Goal3DTool 117 | Topic: goal 118 | Value: true 119 | Views: 120 | Current: 121 | Class: rviz/Orbit 122 | Distance: 9.74612236 123 | Enable Stereo Rendering: 124 | Stereo Eye Separation: 0.0599999987 125 | Stereo Focal Distance: 1 126 | Swap Stereo Eyes: false 127 | Value: false 128 | Focal Point: 129 | X: 2.19087362 130 | Y: 0.484706104 131 | Z: 0.618929148 132 | Focal Shape Fixed Size: true 133 | Focal Shape Size: 0.0500000007 134 | Invert Z Axis: false 135 | Name: Current View 136 | Near Clip Distance: 0.00999999978 137 | Pitch: 1.56979632 138 | Target Frame: 139 | Value: Orbit (rviz) 140 | Yaw: 3.77436399 141 | Saved: 142 | - Class: rviz/Orbit 143 | Distance: 79.2448425 144 | Enable Stereo Rendering: 145 | Stereo Eye Separation: 0.0599999987 146 | Stereo Focal Distance: 1 147 | Swap Stereo Eyes: false 148 | Value: false 149 | Focal Point: 150 | X: -13.6964359 151 | Y: -5.96537924 152 | Z: -19.0381603 153 | Focal Shape Fixed Size: true 154 | Focal Shape Size: 0.0500000007 155 | Invert Z Axis: false 156 | Name: Orbit 157 | Near Clip Distance: 0.00999999978 158 | Pitch: 1.1997968 159 | Target Frame: 160 | Value: Orbit (rviz) 161 | Yaw: 4.55750179 162 | - Angle: -0.00499992538 163 | Class: rviz/TopDownOrtho 164 | Enable Stereo Rendering: 165 | Stereo Eye Separation: 0.0599999987 166 | Stereo Focal Distance: 1 167 | Swap Stereo Eyes: false 168 | Value: false 169 | Invert Z Axis: false 170 | Name: TopDownOrtho 171 | Near Clip Distance: 0.00999999978 172 | Scale: 17.4224415 173 | Target Frame: 174 | Value: TopDownOrtho (rviz) 175 | X: -8.52147961 176 | Y: -7.69360256 177 | - Angle: 0.0150001263 178 | Class: rviz/TopDownOrtho 179 | Enable Stereo Rendering: 180 | Stereo Eye Separation: 0.0599999987 181 | Stereo Focal Distance: 1 182 | Swap Stereo Eyes: false 183 | Value: false 184 | Invert Z Axis: false 185 | Name: TopDownOrtho 186 | Near Clip Distance: 0.00999999978 187 | Scale: 134.654221 188 | Target Frame: 189 | Value: TopDownOrtho (rviz) 190 | X: -15.3867512 191 | Y: -4.14116335 192 | - Angle: -0.0249942318 193 | Class: rviz/TopDownOrtho 194 | Enable Stereo Rendering: 195 | Stereo Eye Separation: 0.0599999987 196 | Stereo Focal Distance: 1 197 | Swap Stereo Eyes: false 198 | Value: false 199 | Invert Z Axis: false 200 | Name: TopDownOrtho 201 | Near Clip Distance: 0.00999999978 202 | Scale: 10.9894609 203 | Target Frame: 204 | Value: TopDownOrtho (rviz) 205 | X: -15.1368361 206 | Y: -11.9202261 207 | Window Geometry: 208 | Displays: 209 | collapsed: false 210 | Height: 1028 211 | Hide Left Dock: false 212 | Hide Right Dock: false 213 | QMainWindow State: 000000ff00000000fd0000000400000000000001a2000003befc020000000bfb000000100044006900730070006c006100790073010000002800000277000000d700fffffffb0000000a0056006900650077007301000002a500000141000000ad00fffffffb0000001200530065006c0065006300740069006f006e00000000000000009b0000006100fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000e3000001c60000006100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065020000037e000000e30000016a00000089fb0000000a0049006d0061006700650200000593000001f50000016a000000ed000000010000010f000002c4fc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f000000ddfc0100000002fb0000000c00430061006d006500720061020000012e00000138000002d9000001a9fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005730000003efc0100000002fb0000000800540069006d00650000000000000005730000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000597000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 214 | Selection: 215 | collapsed: false 216 | Time: 217 | collapsed: false 218 | Tool Properties: 219 | collapsed: false 220 | Views: 221 | collapsed: false 222 | Width: 1855 223 | X: 142 224 | Y: 69 225 | -------------------------------------------------------------------------------- /grid_path_searcher/src/random_complex_generator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace std; 21 | using namespace Eigen; 22 | 23 | ros::Publisher _all_map_pub; 24 | 25 | int _obs_num, _cir_num; 26 | double _x_size, _y_size, _z_size, _init_x, _init_y, _resolution, _sense_rate; 27 | double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h, _w_c_l, _w_c_h; 28 | 29 | bool _has_map = false; 30 | 31 | sensor_msgs::PointCloud2 globalMap_pcd; 32 | pcl::PointCloud cloudMap; 33 | 34 | pcl::search::KdTree kdtreeMap; 35 | vector pointIdxSearch; 36 | vector pointSquaredDistance; 37 | 38 | void RandomMapGenerate() 39 | { 40 | random_device rd; 41 | default_random_engine eng(rd()); 42 | 43 | uniform_real_distribution rand_x = uniform_real_distribution(_x_l, _x_h ); 44 | uniform_real_distribution rand_y = uniform_real_distribution(_y_l, _y_h ); 45 | uniform_real_distribution rand_w = uniform_real_distribution(_w_l, _w_h); 46 | uniform_real_distribution rand_h = uniform_real_distribution(_h_l, _h_h); 47 | 48 | uniform_real_distribution rand_x_circle = uniform_real_distribution(_x_l + 1.0, _x_h - 1.0); 49 | uniform_real_distribution rand_y_circle = uniform_real_distribution(_y_l + 1.0, _y_h - 1.0); 50 | uniform_real_distribution rand_r_circle = uniform_real_distribution(_w_c_l , _w_c_h ); 51 | 52 | uniform_real_distribution rand_roll = uniform_real_distribution(- M_PI, + M_PI); 53 | uniform_real_distribution rand_pitch = uniform_real_distribution(+ M_PI/4.0, + M_PI/2.0); 54 | uniform_real_distribution rand_yaw = uniform_real_distribution(+ M_PI/4.0, + M_PI/2.0); 55 | uniform_real_distribution rand_ellipse_c = uniform_real_distribution(0.5, 2.0); 56 | uniform_real_distribution rand_num = uniform_real_distribution(0.0, 1.0); 57 | 58 | pcl::PointXYZ pt_random; 59 | 60 | for(int i = 0; i < _cir_num; i ++) 61 | { 62 | double x0, y0, z0, R; 63 | std::vector circle_set; 64 | 65 | x0 = rand_x_circle(eng); 66 | y0 = rand_y_circle(eng); 67 | z0 = rand_h(eng) / 2.0; 68 | R = rand_r_circle(eng); 69 | 70 | if(sqrt( pow(x0-_init_x, 2) + pow(y0-_init_y, 2) ) < 2.0 ) 71 | continue; 72 | 73 | double a, b; 74 | a = rand_ellipse_c(eng); 75 | b = rand_ellipse_c(eng); 76 | 77 | double x, y, z; 78 | Vector3d pt3, pt3_rot; 79 | for(double theta = -M_PI; theta < M_PI; theta += 0.025) 80 | { 81 | x = a * cos(theta) * R; 82 | y = b * sin(theta) * R; 83 | z = 0; 84 | pt3 << x, y, z; 85 | circle_set.push_back(pt3); 86 | } 87 | 88 | Matrix3d Rot; 89 | double roll, pitch, yaw; 90 | double alpha, beta, gama; 91 | roll = rand_roll(eng); 92 | pitch = rand_pitch(eng); 93 | yaw = rand_yaw(eng); 94 | 95 | alpha = roll; 96 | beta = pitch; 97 | gama = yaw; 98 | 99 | double p = rand_num(eng); 100 | if(p < 0.5) 101 | { 102 | beta = M_PI / 2.0; 103 | gama = M_PI / 2.0; 104 | } 105 | 106 | Rot << cos(alpha) * cos(gama) - cos(beta) * sin(alpha) * sin(gama), - cos(beta) * cos(gama) * sin(alpha) - cos(alpha) * sin(gama), sin(alpha) * sin(beta), 107 | cos(gama) * sin(alpha) + cos(alpha) * cos(beta) * sin(gama), cos(alpha) * cos(beta) * cos(gama) - sin(alpha) * sin(gama), - cos(alpha) * sin(beta), 108 | sin(beta) * sin(gama), cos(gama) * sin(beta), cos(beta); 109 | 110 | for(auto pt: circle_set) 111 | { 112 | pt3_rot = Rot * pt; 113 | pt_random.x = pt3_rot(0) + x0 + 0.001; 114 | pt_random.y = pt3_rot(1) + y0 + 0.001; 115 | pt_random.z = pt3_rot(2) + z0 + 0.001; 116 | 117 | if(pt_random.z >= 0.0) 118 | cloudMap.points.push_back( pt_random ); 119 | } 120 | } 121 | 122 | bool is_kdtree_empty = false; 123 | if(cloudMap.points.size() > 0) 124 | kdtreeMap.setInputCloud( cloudMap.makeShared() ); 125 | else 126 | is_kdtree_empty = true; 127 | 128 | 129 | for(int i = 0; i < _obs_num; i ++) 130 | { 131 | double x, y, w, h; 132 | x = rand_x(eng); 133 | y = rand_y(eng); 134 | w = rand_w(eng); 135 | 136 | 137 | if(sqrt( pow(x - _init_x, 2) + pow(y - _init_y, 2) ) < 0.8 ) 138 | continue; 139 | 140 | pcl::PointXYZ searchPoint(x, y, (_h_l + _h_h)/2.0); 141 | pointIdxSearch.clear(); 142 | pointSquaredDistance.clear(); 143 | 144 | if(is_kdtree_empty == false) 145 | { 146 | if ( kdtreeMap.nearestKSearch (searchPoint, 1, pointIdxSearch, pointSquaredDistance) > 0 ) 147 | { 148 | if(sqrt(pointSquaredDistance[0]) < 1.0 ) 149 | continue; 150 | } 151 | } 152 | 153 | x = floor(x/_resolution) * _resolution + _resolution / 2.0; 154 | y = floor(y/_resolution) * _resolution + _resolution / 2.0; 155 | 156 | int widNum = ceil(w/_resolution); 157 | for(int r = -widNum/2.0; r < widNum/2.0; r ++ ) 158 | { 159 | for(int s = -widNum/2.0; s < widNum/2.0; s ++ ) 160 | { 161 | h = rand_h(eng); 162 | int heiNum = 2.0 * ceil(h/_resolution); 163 | for(int t = 0; t < heiNum; t ++ ){ 164 | pt_random.x = x + (r+0.0) * _resolution + 0.001; 165 | pt_random.y = y + (s+0.0) * _resolution + 0.001; 166 | pt_random.z = (t+0.0) * _resolution * 0.5 + 0.001; 167 | cloudMap.points.push_back( pt_random ); 168 | } 169 | } 170 | } 171 | } 172 | 173 | cloudMap.width = cloudMap.points.size(); 174 | cloudMap.height = 1; 175 | cloudMap.is_dense = true; 176 | 177 | _has_map = true; 178 | 179 | pcl::toROSMsg(cloudMap, globalMap_pcd); 180 | globalMap_pcd.header.frame_id = "world"; 181 | } 182 | 183 | void pubSensedPoints() 184 | { 185 | if( !_has_map ) return; 186 | 187 | _all_map_pub.publish(globalMap_pcd); 188 | } 189 | 190 | int main (int argc, char** argv) 191 | { 192 | ros::init (argc, argv, "random_complex_scene"); 193 | ros::NodeHandle n( "~" ); 194 | 195 | _all_map_pub = n.advertise("global_map", 1); 196 | 197 | n.param("init_state_x", _init_x, 0.0); 198 | n.param("init_state_y", _init_y, 0.0); 199 | 200 | n.param("map/x_size", _x_size, 50.0); 201 | n.param("map/y_size", _y_size, 50.0); 202 | n.param("map/z_size", _z_size, 5.0 ); 203 | 204 | n.param("map/obs_num", _obs_num, 30); 205 | n.param("map/circle_num", _cir_num, 30); 206 | n.param("map/resolution", _resolution, 0.2); 207 | 208 | n.param("ObstacleShape/lower_rad", _w_l, 0.3); 209 | n.param("ObstacleShape/upper_rad", _w_h, 0.8); 210 | n.param("ObstacleShape/lower_hei", _h_l, 3.0); 211 | n.param("ObstacleShape/upper_hei", _h_h, 7.0); 212 | 213 | n.param("CircleShape/lower_circle_rad", _w_c_l, 0.3); 214 | n.param("CircleShape/upper_circle_rad", _w_c_h, 0.8); 215 | 216 | n.param("sensing/rate", _sense_rate, 1.0); 217 | 218 | _x_l = - _x_size / 2.0; 219 | _x_h = + _x_size / 2.0; 220 | 221 | _y_l = - _y_size / 2.0; 222 | _y_h = + _y_size / 2.0; 223 | 224 | RandomMapGenerate(); 225 | ros::Rate loop_rate(_sense_rate); 226 | while (ros::ok()) 227 | { 228 | pubSensedPoints(); 229 | ros::spinOnce(); 230 | loop_rate.sleep(); 231 | } 232 | } 233 | -------------------------------------------------------------------------------- /grid_path_searcher/src/demo_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "Astar_searcher.h" 18 | #include "JPS_searcher.h" 19 | #include "backward.hpp" 20 | 21 | using namespace std; 22 | using namespace Eigen; 23 | 24 | #define _use_jps 1 //0 -> 只使用Ａ*, 1 -> 使用JPS 25 | 26 | namespace backward { 27 | backward::SignalHandling sh; 28 | } 29 | 30 | 31 | double _resolution, _inv_resolution, _cloud_margin; 32 | 33 | double _x_size, _y_size, _z_size; 34 | 35 | bool _has_map = false; 36 | 37 | Vector3d _start_pt; 38 | 39 | Vector3d _map_lower, _map_upper; 40 | 41 | int _max_x_id, _max_y_id, _max_z_id; 42 | 43 | 44 | ros::Subscriber _map_sub, _pts_sub; 45 | ros::Publisher _grid_path_vis_pub, _visited_nodes_vis_pub, _grid_map_vis_pub; 46 | 47 | AstarPathFinder * _astar_path_finder = new AstarPathFinder(); 48 | JPSPathFinder *_jps_path_finder = new JPSPathFinder(); 49 | 50 | void rcvWaypointsCallback(const nav_msgs::Path & wp); 51 | 52 | void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map); 53 | 54 | void visGridPath( vector nodes, bool is_use_jps ); 55 | 56 | void visVisitedNode( vector nodes ); 57 | 58 | void pathFinding(const Vector3d start_pt, const Vector3d target_pt); 59 | 60 | void rcvWaypointsCallback(const nav_msgs::Path & wp) 61 | { 62 | if( wp.poses[0].pose.position.z < 0.0 || _has_map == false ) 63 | return; 64 | 65 | Vector3d target_pt; 66 | target_pt << wp.poses[0].pose.position.x, 67 | wp.poses[0].pose.position.y, 68 | wp.poses[0].pose.position.z; 69 | 70 | ROS_INFO("[node] receive the planning target"); 71 | pathFinding(_start_pt, target_pt); 72 | } 73 | 74 | void rcvPointCloudCallBack(const sensor_msgs::PointCloud2 & pointcloud_map) 75 | { 76 | if(_has_map ) return; 77 | 78 | pcl::PointCloud cloud; 79 | pcl::PointCloud cloud_vis; 80 | sensor_msgs::PointCloud2 map_vis; 81 | 82 | pcl::fromROSMsg(pointcloud_map, cloud); 83 | 84 | if( (int)cloud.points.size() == 0 ) return; 85 | 86 | pcl::PointXYZ pt; 87 | for (int idx = 0; idx < (int)cloud.points.size(); idx++) 88 | { 89 | pt = cloud.points[idx]; 90 | 91 | _astar_path_finder->setObs(pt.x, pt.y, pt.z); 92 | _jps_path_finder->setObs(pt.x, pt.y, pt.z); 93 | 94 | Vector3d cor_round = _astar_path_finder->coordRounding(Vector3d(pt.x, pt.y, pt.z)); 95 | pt.x = cor_round(0); 96 | pt.y = cor_round(1); 97 | pt.z = cor_round(2); 98 | cloud_vis.points.push_back(pt); 99 | } 100 | 101 | cloud_vis.width = cloud_vis.points.size(); 102 | cloud_vis.height = 1; 103 | cloud_vis.is_dense = true; 104 | 105 | pcl::toROSMsg(cloud_vis, map_vis); 106 | 107 | map_vis.header.frame_id = "/world"; 108 | _grid_map_vis_pub.publish(map_vis); 109 | 110 | _has_map = true; 111 | } 112 | 113 | 114 | void pathFinding(const Vector3d start_pt, const Vector3d target_pt) 115 | { 116 | _astar_path_finder->AstarGraphSearch(start_pt, target_pt); 117 | 118 | auto grid_path = _astar_path_finder->getPath(); 119 | auto visited_nodes = _astar_path_finder->getVisitedNodes(); 120 | 121 | visGridPath (grid_path, false); 122 | 123 | visVisitedNode(visited_nodes); 124 | 125 | _astar_path_finder->resetUsedGrids(); 126 | 127 | if(_use_jps) 128 | { 129 | // 调用JPS算法 130 | _jps_path_finder->JPSGraphSearch(start_pt, target_pt); 131 | 132 | //获得最优路径和访问的节点 133 | auto visited_nodes = _jps_path_finder->getVisitedNodes(); 134 | auto grid_path = _jps_path_finder->getPath(); 135 | 136 | //可视化 137 | visGridPath(grid_path, _use_jps); 138 | visVisitedNode(visited_nodes); 139 | 140 | //重置栅格地图,为下次调用做准备 141 | _jps_path_finder->resetUsedGrids(); 142 | } 143 | 144 | 145 | 146 | } 147 | 148 | int main(int argc, char** argv) 149 | { 150 | ros::init(argc, argv, "demo_node"); 151 | ros::NodeHandle nh("~"); 152 | 153 | _map_sub = nh.subscribe( "map", 1, rcvPointCloudCallBack ); 154 | _pts_sub = nh.subscribe( "waypoints", 1, rcvWaypointsCallback ); 155 | 156 | _grid_map_vis_pub = nh.advertise("grid_map_vis", 1); 157 | _grid_path_vis_pub = nh.advertise("grid_path_vis", 1); 158 | _visited_nodes_vis_pub = nh.advertise("visited_nodes_vis",1); 159 | 160 | 161 | nh.param("map/cloud_margin", _cloud_margin, 0.0); 162 | nh.param("map/resolution", _resolution, 0.2); 163 | 164 | nh.param("map/x_size", _x_size, 50.0); 165 | nh.param("map/y_size", _y_size, 50.0); 166 | nh.param("map/z_size", _z_size, 5.0 ); 167 | 168 | nh.param("planning/start_x", _start_pt(0), 0.0); 169 | nh.param("planning/start_y", _start_pt(1), 0.0); 170 | nh.param("planning/start_z", _start_pt(2), 0.0); 171 | 172 | _map_lower << - _x_size/2.0, - _y_size/2.0, 0.0; 173 | _map_upper << + _x_size/2.0, + _y_size/2.0, _z_size; 174 | 175 | _inv_resolution = 1.0 / _resolution; 176 | 177 | _max_x_id = (int)(_x_size * _inv_resolution); 178 | _max_y_id = (int)(_y_size * _inv_resolution); 179 | _max_z_id = (int)(_z_size * _inv_resolution); 180 | 181 | _astar_path_finder = new AstarPathFinder(); 182 | 183 | _astar_path_finder -> initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id); 184 | 185 | _jps_path_finder = new JPSPathFinder(); 186 | _jps_path_finder->initGridMap(_resolution, _map_lower, _map_upper, _max_x_id, _max_y_id, _max_z_id); 187 | 188 | ros::Rate rate(100); 189 | bool status = ros::ok(); 190 | while(status) 191 | { 192 | ros::spinOnce(); 193 | status = ros::ok(); 194 | rate.sleep(); 195 | } 196 | 197 | delete _astar_path_finder; 198 | delete _jps_path_finder; 199 | return 0; 200 | } 201 | 202 | void visGridPath( vector nodes, bool is_use_jps ) 203 | { 204 | visualization_msgs::Marker node_vis; 205 | node_vis.header.frame_id = "world"; 206 | node_vis.header.stamp = ros::Time::now(); 207 | 208 | if(is_use_jps) 209 | node_vis.ns = "demo_node/jps_path"; 210 | else 211 | node_vis.ns = "demo_node/astar_path"; 212 | 213 | node_vis.type = visualization_msgs::Marker::CUBE_LIST; 214 | node_vis.action = visualization_msgs::Marker::ADD; 215 | node_vis.id = 0; 216 | 217 | node_vis.pose.orientation.x = 0.0; 218 | node_vis.pose.orientation.y = 0.0; 219 | node_vis.pose.orientation.z = 0.0; 220 | node_vis.pose.orientation.w = 1.0; 221 | 222 | if(is_use_jps){ 223 | node_vis.color.a = 1.0; //jps black 224 | node_vis.color.r = 0.0; 225 | node_vis.color.g = 0.0; 226 | node_vis.color.b = 0.0; 227 | } 228 | else{ 229 | node_vis.color.a = 1.0; //astar white 230 | node_vis.color.r = 1.0; 231 | node_vis.color.g = 1.0; 232 | node_vis.color.b = 1.0; 233 | } 234 | 235 | 236 | node_vis.scale.x = _resolution; 237 | node_vis.scale.y = _resolution; 238 | node_vis.scale.z = _resolution; 239 | 240 | geometry_msgs::Point pt; 241 | for(int i = 0; i < int(nodes.size()); i++) 242 | { 243 | Vector3d coord = nodes[i]; 244 | pt.x = coord(0); 245 | pt.y = coord(1); 246 | pt.z = coord(2); 247 | 248 | node_vis.points.push_back(pt); 249 | } 250 | 251 | _grid_path_vis_pub.publish(node_vis); 252 | } 253 | 254 | void visVisitedNode( vector nodes ) 255 | { 256 | visualization_msgs::Marker node_vis; 257 | node_vis.header.frame_id = "world"; 258 | node_vis.header.stamp = ros::Time::now(); 259 | node_vis.ns = "demo_node/expanded_nodes"; 260 | node_vis.type = visualization_msgs::Marker::CUBE_LIST; 261 | node_vis.action = visualization_msgs::Marker::ADD; 262 | node_vis.id = 0; 263 | 264 | node_vis.pose.orientation.x = 0.0; 265 | node_vis.pose.orientation.y = 0.0; 266 | node_vis.pose.orientation.z = 0.0; 267 | node_vis.pose.orientation.w = 1.0; 268 | node_vis.color.a = 0.5; 269 | node_vis.color.r = 0.0; 270 | node_vis.color.g = 0.0; 271 | node_vis.color.b = 1.0; 272 | 273 | node_vis.scale.x = _resolution; 274 | node_vis.scale.y = _resolution; 275 | node_vis.scale.z = _resolution; 276 | 277 | geometry_msgs::Point pt; 278 | for(int i = 0; i < int(nodes.size()); i++) 279 | { 280 | Vector3d coord = nodes[i]; 281 | pt.x = coord(0); 282 | pt.y = coord(1); 283 | pt.z = coord(2); 284 | 285 | node_vis.points.push_back(pt); 286 | } 287 | 288 | _visited_nodes_vis_pub.publish(node_vis); 289 | } 290 | -------------------------------------------------------------------------------- /waypoint_generator/src/waypoint_generator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "sample_waypoints.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | using bfmt = boost::format; 17 | 18 | ros::Publisher pub1; 19 | ros::Publisher pub2; 20 | ros::Publisher pub3; 21 | string waypoint_type = string("manual"); 22 | bool is_odom_ready; 23 | nav_msgs::Odometry odom; 24 | nav_msgs::Path waypoints; 25 | 26 | 27 | std::deque waypointSegments; 28 | ros::Time trigged_time; 29 | 30 | void load_seg(ros::NodeHandle& nh, int segid, const ros::Time& time_base) { 31 | std::string seg_str = boost::str(bfmt("seg%d/") % segid); 32 | double yaw; 33 | double time_from_start; 34 | ROS_INFO("Getting segment %d", segid); 35 | ROS_ASSERT(nh.getParam(seg_str + "yaw", yaw)); 36 | ROS_ASSERT_MSG((yaw > -3.1499999) && (yaw < 3.14999999), "yaw=%.3f", yaw); 37 | ROS_ASSERT(nh.getParam(seg_str + "time_from_start", time_from_start)); 38 | ROS_ASSERT(time_from_start >= 0.0); 39 | 40 | std::vector ptx; 41 | std::vector pty; 42 | std::vector ptz; 43 | 44 | ROS_ASSERT(nh.getParam(seg_str + "x", ptx)); 45 | ROS_ASSERT(nh.getParam(seg_str + "y", pty)); 46 | ROS_ASSERT(nh.getParam(seg_str + "z", ptz)); 47 | 48 | ROS_ASSERT(ptx.size()); 49 | ROS_ASSERT(ptx.size() == pty.size() && ptx.size() == ptz.size()); 50 | 51 | nav_msgs::Path path_msg; 52 | 53 | path_msg.header.stamp = time_base + ros::Duration(time_from_start); 54 | 55 | double baseyaw = tf::getYaw(odom.pose.pose.orientation); 56 | 57 | for (size_t k = 0; k < ptx.size(); ++k) { 58 | geometry_msgs::PoseStamped pt; 59 | pt.pose.orientation = tf::createQuaternionMsgFromYaw(baseyaw + yaw); 60 | Eigen::Vector2d dp(ptx.at(k), pty.at(k)); 61 | Eigen::Vector2d rdp; 62 | rdp.x() = std::cos(-baseyaw-yaw)*dp.x() + std::sin(-baseyaw-yaw)*dp.y(); 63 | rdp.y() =-std::sin(-baseyaw-yaw)*dp.x() + std::cos(-baseyaw-yaw)*dp.y(); 64 | pt.pose.position.x = rdp.x() + odom.pose.pose.position.x; 65 | pt.pose.position.y = rdp.y() + odom.pose.pose.position.y; 66 | pt.pose.position.z = ptz.at(k) + odom.pose.pose.position.z; 67 | path_msg.poses.push_back(pt); 68 | } 69 | 70 | waypointSegments.push_back(path_msg); 71 | } 72 | 73 | void load_waypoints(ros::NodeHandle& nh, const ros::Time& time_base) { 74 | int seg_cnt = 0; 75 | waypointSegments.clear(); 76 | ROS_ASSERT(nh.getParam("segment_cnt", seg_cnt)); 77 | for (int i = 0; i < seg_cnt; ++i) { 78 | load_seg(nh, i, time_base); 79 | if (i > 0) { 80 | ROS_ASSERT(waypointSegments[i - 1].header.stamp < waypointSegments[i].header.stamp); 81 | } 82 | } 83 | ROS_INFO("Overall load %zu segments", waypointSegments.size()); 84 | } 85 | 86 | void publish_waypoints() { 87 | waypoints.header.frame_id = std::string("world"); 88 | waypoints.header.stamp = ros::Time::now(); 89 | pub1.publish(waypoints); 90 | geometry_msgs::PoseStamped init_pose; 91 | init_pose.header = odom.header; 92 | init_pose.pose = odom.pose.pose; 93 | waypoints.poses.insert(waypoints.poses.begin(), init_pose); 94 | 95 | waypoints.poses.clear(); 96 | } 97 | 98 | void publish_waypoints_vis() { 99 | nav_msgs::Path wp_vis = waypoints; 100 | geometry_msgs::PoseArray poseArray; 101 | poseArray.header.frame_id = std::string("world"); 102 | poseArray.header.stamp = ros::Time::now(); 103 | 104 | { 105 | geometry_msgs::Pose init_pose; 106 | init_pose = odom.pose.pose; 107 | poseArray.poses.push_back(init_pose); 108 | } 109 | 110 | for (auto it = waypoints.poses.begin(); it != waypoints.poses.end(); ++it) { 111 | geometry_msgs::Pose p; 112 | p = it->pose; 113 | poseArray.poses.push_back(p); 114 | } 115 | pub2.publish(poseArray); 116 | } 117 | 118 | void odom_callback(const nav_msgs::Odometry::ConstPtr& msg) { 119 | is_odom_ready = true; 120 | odom = *msg; 121 | 122 | if (waypointSegments.size()) { 123 | ros::Time expected_time = waypointSegments.front().header.stamp; 124 | if (odom.header.stamp >= expected_time) { 125 | waypoints = waypointSegments.front(); 126 | 127 | std::stringstream ss; 128 | ss << bfmt("Series send %.3f from start:\n") % trigged_time.toSec(); 129 | for (auto& pose_stamped : waypoints.poses) { 130 | ss << bfmt("P[%.2f, %.2f, %.2f] q(%.2f,%.2f,%.2f,%.2f)") % 131 | pose_stamped.pose.position.x % pose_stamped.pose.position.y % 132 | pose_stamped.pose.position.z % pose_stamped.pose.orientation.w % 133 | pose_stamped.pose.orientation.x % pose_stamped.pose.orientation.y % 134 | pose_stamped.pose.orientation.z << std::endl; 135 | } 136 | ROS_INFO_STREAM(ss.str()); 137 | 138 | publish_waypoints_vis(); 139 | publish_waypoints(); 140 | 141 | waypointSegments.pop_front(); 142 | } 143 | } 144 | } 145 | 146 | 147 | void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) { 148 | 149 | trigged_time = ros::Time::now(); 150 | 151 | 152 | ros::NodeHandle n("~"); 153 | n.param("waypoint_type", waypoint_type, string("manual")); 154 | 155 | if (waypoint_type == string("circle")) { 156 | waypoints = circle(); 157 | publish_waypoints_vis(); 158 | publish_waypoints(); 159 | } else if (waypoint_type == string("eight")) { 160 | waypoints = eight(); 161 | publish_waypoints_vis(); 162 | publish_waypoints(); 163 | } else if (waypoint_type == string("point")) { 164 | waypoints = point(); 165 | publish_waypoints_vis(); 166 | publish_waypoints(); 167 | } else if (waypoint_type == string("series")) { 168 | load_waypoints(n, trigged_time); 169 | 170 | 171 | } else if (waypoint_type == string("manual-lonely-waypoint")) { 172 | if (msg->pose.position.z >= 0) { 173 | 174 | geometry_msgs::PoseStamped pt = *msg; 175 | waypoints.poses.clear(); 176 | waypoints.poses.push_back(pt); 177 | publish_waypoints_vis(); 178 | publish_waypoints(); 179 | } else { 180 | ROS_WARN("[waypoint_generator] invalid goal in manual-lonely-waypoint mode."); 181 | } 182 | } else { 183 | if (msg->pose.position.z > 0) { 184 | 185 | geometry_msgs::PoseStamped pt = *msg; 186 | if (waypoint_type == string("noyaw")) { 187 | double yaw = tf::getYaw(odom.pose.pose.orientation); 188 | pt.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); 189 | } 190 | waypoints.poses.push_back(pt); 191 | publish_waypoints_vis(); 192 | } else if (msg->pose.position.z > -1.0) { 193 | 194 | if (waypoints.poses.size() >= 1) { 195 | waypoints.poses.erase(std::prev(waypoints.poses.end())); 196 | } 197 | publish_waypoints_vis(); 198 | } else { 199 | 200 | if (waypoints.poses.size() >= 1) { 201 | publish_waypoints_vis(); 202 | publish_waypoints(); 203 | } 204 | } 205 | } 206 | } 207 | 208 | void traj_start_trigger_callback(const geometry_msgs::PoseStamped& msg) { 209 | if (!is_odom_ready) { 210 | ROS_ERROR("[waypoint_generator] No odom!"); 211 | return; 212 | } 213 | 214 | ROS_WARN("[waypoint_generator] Trigger!"); 215 | trigged_time = odom.header.stamp; 216 | ROS_ASSERT(trigged_time > ros::Time(0)); 217 | 218 | ros::NodeHandle n("~"); 219 | n.param("waypoint_type", waypoint_type, string("manual")); 220 | 221 | ROS_ERROR_STREAM("Pattern " << waypoint_type << " generated!"); 222 | if (waypoint_type == string("free")) { 223 | waypoints = point(); 224 | publish_waypoints_vis(); 225 | publish_waypoints(); 226 | } else if (waypoint_type == string("circle")) { 227 | waypoints = circle(); 228 | publish_waypoints_vis(); 229 | publish_waypoints(); 230 | } else if (waypoint_type == string("eight")) { 231 | waypoints = eight(); 232 | publish_waypoints_vis(); 233 | publish_waypoints(); 234 | } else if (waypoint_type == string("point")) { 235 | waypoints = point(); 236 | publish_waypoints_vis(); 237 | publish_waypoints(); 238 | } else if (waypoint_type == string("series")) { 239 | load_waypoints(n, trigged_time); 240 | } 241 | } 242 | 243 | int main(int argc, char** argv) { 244 | ros::init(argc, argv, "waypoint_generator"); 245 | ros::NodeHandle n("~"); 246 | n.param("waypoint_type", waypoint_type, string("manual")); 247 | ros::Subscriber sub1 = n.subscribe("odom", 10, odom_callback); 248 | ros::Subscriber sub2 = n.subscribe("goal", 10, goal_callback); 249 | ros::Subscriber sub3 = n.subscribe("traj_start_trigger", 10, traj_start_trigger_callback); 250 | pub1 = n.advertise("waypoints", 50); 251 | pub2 = n.advertise("waypoints_vis", 10); 252 | 253 | trigged_time = ros::Time(0); 254 | 255 | ros::spin(); 256 | return 0; 257 | } 258 | -------------------------------------------------------------------------------- /grid_path_searcher/src/JPS_searcher.cpp: -------------------------------------------------------------------------------- 1 | #include "JPS_searcher.h" 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | 6 | void JPSPathFinder::JPSGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt) 7 | { 8 | ros::Time time_1 = ros::Time::now(); 9 | 10 | //起点和终点的索引向量 11 | Vector3i start_idx = coord2gridIndex(start_pt); 12 | Vector3i end_idx = coord2gridIndex(end_pt); 13 | goalIdx = end_idx; 14 | 15 | //起点和终点的位置向量 16 | start_pt = gridIndex2coord(start_idx); 17 | end_pt = gridIndex2coord(end_idx); 18 | 19 | //初始化起点和终点的节点(指针) 20 | GridNodePtr startPtr = GridNodeMap[start_idx(0)][start_idx(1)][start_idx(2)]; 21 | GridNodePtr endPtr = GridNodeMap[end_idx(0)][end_idx(1)][end_idx(2)]; 22 | 23 | openSet.clear(); //使用STL库中的multimap维护openset 24 | 25 | GridNodePtr currentPtr = NULL; // currentPtr代表openset中f(n)最小的节点 26 | GridNodePtr neighborPtr = NULL; 27 | 28 | startPtr->gScore = 0; 29 | startPtr->fScore = getHeu2(startPtr, endPtr); 30 | startPtr->id = 1; //标记起始节点为未访问 31 | openSet.insert(make_pair(startPtr->fScore, startPtr)); //将起始节点加入openset 32 | 33 | double tentative_gScore; 34 | vector neighborPtrSets; 35 | vector edgeCostSets; 36 | 37 | //主循环 38 | while (!openSet.empty()) 39 | { 40 | 41 | std::multimap::iterator itFoundmin; 42 | for (itFoundmin = openSet.begin(); itFoundmin != openSet.end(); itFoundmin++) 43 | { 44 | if (itFoundmin->second->id == 1) //说明该节点没被访问过 45 | { 46 | currentPtr = itFoundmin->second; 47 | currentPtr->id = -1; //标记当前节点为已访问状态 48 | break; 49 | } 50 | } 51 | 52 | //找到Goal,返回 53 | if (currentPtr->index == goalIdx) 54 | { 55 | ros::Time time_2 = ros::Time::now(); 56 | terminatePtr = currentPtr; 57 | ROS_WARN("[Better_A*_JPS_Black] succeed ! Time consumed : %f ms, path cost : %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution); 58 | return; 59 | } 60 | 61 | //得到当前节点的继承 62 | JPSGetSucc(currentPtr, neighborPtrSets, edgeCostSets); 63 | 64 | //处理发现的neighbor 65 | for (int i = 0; i < (int)neighborPtrSets.size(); i++) 66 | { 67 | 68 | neighborPtr = neighborPtrSets[i]; 69 | if (neighborPtr->id == 0) //发现一个新节点 70 | { 71 | neighborPtr->id = 1; //标记该节点为未访问状态 72 | neighborPtr->cameFrom = currentPtr; 73 | neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i]; 74 | neighborPtr->fScore = neighborPtr->gScore + getHeu2(neighborPtr, endPtr); 75 | openSet.insert(make_pair(neighborPtr->fScore, neighborPtr)); 76 | continue; 77 | } 78 | else if(neighborPtr->id == 1) //该节点已经在openSet内 79 | { 80 | tentative_gScore = currentPtr->gScore + edgeCostSets[i]; 81 | 82 | if(tentative_gScore <= neighborPtr->gScore) //当前路线的g(n) <= openSet中的g(n), 需要更新 83 | { 84 | neighborPtr->gScore = tentative_gScore; 85 | neighborPtr->fScore = neighborPtr->gScore + getHeu2(neighborPtr, endPtr); 86 | neighborPtr->cameFrom = currentPtr; 87 | for (int i = 0; i < 3; i++) //计算新的扩展方向 88 | { 89 | neighborPtr->dir(i) = neighborPtr->index(i) - currentPtr->index(i); 90 | if (neighborPtr->dir(i) != 0) 91 | neighborPtr->dir(i) /= abs(neighborPtr->dir(i)); 92 | } 93 | } 94 | } 95 | 96 | } 97 | } 98 | 99 | //搜索失败 100 | ros::Time time_2 = ros::Time::now(); 101 | if ((time_2 - time_1).toSec() > 0.1) 102 | ROS_WARN("[JPS] failed, Time consume in JPS path finding is %f", (time_2 - time_1).toSec()); 103 | } 104 | 105 | inline void JPSPathFinder::JPSGetSucc(GridNodePtr currentPtr, vector &neighborPtrSets, vector &edgeCostSets) 106 | { 107 | neighborPtrSets.clear(); 108 | edgeCostSets.clear(); 109 | const int norm1 = abs(currentPtr->dir(0)) + abs(currentPtr->dir(1)) + abs(currentPtr->dir(2)); 110 | 111 | int num_neib = jn3d->nsz[norm1][0]; 112 | int num_fneib = jn3d->nsz[norm1][1]; 113 | int id = (currentPtr->dir(0) + 1) + 3 * (currentPtr->dir(1) + 1) + 9 * (currentPtr->dir(2) + 1); 114 | 115 | for (int dev = 0; dev < num_neib + num_fneib; ++dev) 116 | { 117 | Vector3i neighborIdx; 118 | Vector3i expandDir; 119 | 120 | if (dev < num_neib) 121 | { 122 | expandDir(0) = jn3d->ns[id][0][dev]; 123 | expandDir(1) = jn3d->ns[id][1][dev]; 124 | expandDir(2) = jn3d->ns[id][2][dev]; 125 | 126 | if (!jump(currentPtr->index, expandDir, neighborIdx)) 127 | continue; 128 | } 129 | else 130 | { 131 | int nx = currentPtr->index(0) + jn3d->f1[id][0][dev - num_neib]; 132 | int ny = currentPtr->index(1) + jn3d->f1[id][1][dev - num_neib]; 133 | int nz = currentPtr->index(2) + jn3d->f1[id][2][dev - num_neib]; 134 | 135 | if (isOccupied(nx, ny, nz)) 136 | { 137 | expandDir(0) = jn3d->f2[id][0][dev - num_neib]; 138 | expandDir(1) = jn3d->f2[id][1][dev - num_neib]; 139 | expandDir(2) = jn3d->f2[id][2][dev - num_neib]; 140 | 141 | if (!jump(currentPtr->index, expandDir, neighborIdx)) 142 | continue; 143 | } 144 | else 145 | continue; 146 | } 147 | 148 | GridNodePtr nodePtr = GridNodeMap[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)]; 149 | nodePtr->dir = expandDir; 150 | 151 | neighborPtrSets.push_back(nodePtr); 152 | edgeCostSets.push_back( //当前节点与邻居节点之间的cost 153 | sqrt( 154 | (neighborIdx(0) - currentPtr->index(0)) * (neighborIdx(0) - currentPtr->index(0)) + 155 | (neighborIdx(1) - currentPtr->index(1)) * (neighborIdx(1) - currentPtr->index(1)) + 156 | (neighborIdx(2) - currentPtr->index(2)) * (neighborIdx(2) - currentPtr->index(2)))); 157 | } 158 | } 159 | 160 | bool JPSPathFinder::jump(const Vector3i &curIdx, const Vector3i &expDir, Vector3i &neiIdx) 161 | { 162 | neiIdx = curIdx + expDir; 163 | 164 | if (!isFree(neiIdx)) 165 | return false; 166 | 167 | if (neiIdx == goalIdx) 168 | return true; 169 | 170 | if (hasForced(neiIdx, expDir)) 171 | return true; 172 | 173 | const int id = (expDir(0) + 1) + 3 * (expDir(1) + 1) + 9 * (expDir(2) + 1); 174 | const int norm1 = abs(expDir(0)) + abs(expDir(1)) + abs(expDir(2)); 175 | int num_neib = jn3d->nsz[norm1][0]; 176 | 177 | for (int k = 0; k < num_neib - 1; ++k) 178 | { 179 | Vector3i newNeiIdx; 180 | Vector3i newDir(jn3d->ns[id][0][k], jn3d->ns[id][1][k], jn3d->ns[id][2][k]); 181 | if (jump(neiIdx, newDir, newNeiIdx)) 182 | return true; 183 | } 184 | 185 | return jump(neiIdx, expDir, neiIdx); 186 | } 187 | 188 | inline bool JPSPathFinder::hasForced(const Vector3i &idx, const Vector3i &dir) 189 | { 190 | int norm1 = abs(dir(0)) + abs(dir(1)) + abs(dir(2)); 191 | int id = (dir(0) + 1) + 3 * (dir(1) + 1) + 9 * (dir(2) + 1); 192 | 193 | switch (norm1) 194 | { 195 | case 1: 196 | // 1-d move, check 8 neighbors 197 | for (int fn = 0; fn < 8; ++fn) 198 | { 199 | int nx = idx(0) + jn3d->f1[id][0][fn]; 200 | int ny = idx(1) + jn3d->f1[id][1][fn]; 201 | int nz = idx(2) + jn3d->f1[id][2][fn]; 202 | if (isOccupied(nx, ny, nz)) 203 | return true; 204 | } 205 | return false; 206 | 207 | case 2: 208 | // 2-d move, check 8 neighbors 209 | for (int fn = 0; fn < 8; ++fn) 210 | { 211 | int nx = idx(0) + jn3d->f1[id][0][fn]; 212 | int ny = idx(1) + jn3d->f1[id][1][fn]; 213 | int nz = idx(2) + jn3d->f1[id][2][fn]; 214 | if (isOccupied(nx, ny, nz)) 215 | return true; 216 | } 217 | return false; 218 | 219 | case 3: 220 | // 3-d move, check 6 neighbors 221 | for (int fn = 0; fn < 6; ++fn) 222 | { 223 | int nx = idx(0) + jn3d->f1[id][0][fn]; 224 | int ny = idx(1) + jn3d->f1[id][1][fn]; 225 | int nz = idx(2) + jn3d->f1[id][2][fn]; 226 | if (isOccupied(nx, ny, nz)) 227 | return true; 228 | } 229 | return false; 230 | 231 | default: 232 | return false; 233 | } 234 | } 235 | 236 | inline bool JPSPathFinder::isOccupied(const Eigen::Vector3i &index) const 237 | { 238 | return isOccupied(index(0), index(1), index(2)); 239 | } 240 | 241 | inline bool JPSPathFinder::isFree(const Eigen::Vector3i &index) const 242 | { 243 | return isFree(index(0), index(1), index(2)); 244 | } 245 | 246 | inline bool JPSPathFinder::isOccupied(const int &idx_x, const int &idx_y, const int &idx_z) const 247 | { 248 | return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 249 | (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1)); 250 | } 251 | 252 | inline bool JPSPathFinder::isFree(const int &idx_x, const int &idx_y, const int &idx_z) const 253 | { 254 | return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 255 | (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] < 1)); 256 | } 257 | -------------------------------------------------------------------------------- /rviz_plugins/src/multi_probmap_display.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include "rviz/frame_manager.h" 15 | #include "rviz/ogre_helpers/grid.h" 16 | #include "rviz/properties/float_property.h" 17 | #include "rviz/properties/int_property.h" 18 | #include "rviz/properties/property.h" 19 | #include "rviz/properties/quaternion_property.h" 20 | #include "rviz/properties/ros_topic_property.h" 21 | #include "rviz/properties/vector_property.h" 22 | #include "rviz/validate_floats.h" 23 | #include "rviz/display_context.h" 24 | 25 | #include "multi_probmap_display.h" 26 | 27 | namespace rviz 28 | { 29 | 30 | MultiProbMapDisplay::MultiProbMapDisplay() 31 | : Display() 32 | , loaded_( false ) 33 | , new_map_(false) 34 | { 35 | topic_property_ = new RosTopicProperty( "Topic", "", 36 | QString::fromStdString( ros::message_traits::datatype() ), 37 | "multi_map_server::MultiOccupancyGrid topic to subscribe to.", 38 | this, SLOT( updateTopic() )); 39 | 40 | draw_under_property_ = new Property( "Draw Behind", false, 41 | "Rendering option, controls whether or not the map is always" 42 | " drawn behind everything else.", 43 | this, SLOT( updateDrawUnder() )); 44 | } 45 | 46 | MultiProbMapDisplay::~MultiProbMapDisplay() 47 | { 48 | unsubscribe(); 49 | clear(); 50 | } 51 | 52 | void MultiProbMapDisplay::onInitialize() 53 | { 54 | } 55 | 56 | void MultiProbMapDisplay::onEnable() 57 | { 58 | subscribe(); 59 | } 60 | 61 | void MultiProbMapDisplay::onDisable() 62 | { 63 | unsubscribe(); 64 | clear(); 65 | } 66 | 67 | void MultiProbMapDisplay::subscribe() 68 | { 69 | if ( !isEnabled() ) 70 | { 71 | return; 72 | } 73 | 74 | if( !topic_property_->getTopic().isEmpty() ) 75 | { 76 | try 77 | { 78 | map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &MultiProbMapDisplay::incomingMap, this ); 79 | setStatus( StatusProperty::Ok, "Topic", "OK" ); 80 | } 81 | catch( ros::Exception& e ) 82 | { 83 | setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); 84 | } 85 | } 86 | } 87 | 88 | void MultiProbMapDisplay::unsubscribe() 89 | { 90 | map_sub_.shutdown(); 91 | } 92 | 93 | void MultiProbMapDisplay::updateDrawUnder() 94 | { 95 | bool draw_under = draw_under_property_->getValue().toBool(); 96 | 97 | for (unsigned int k = 0; k < material_.size(); k++) 98 | material_[k]->setDepthWriteEnabled( !draw_under ); 99 | 100 | for (unsigned int k = 0; k < manual_object_.size(); k++) 101 | { 102 | if( draw_under ) 103 | manual_object_[k]->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); 104 | else 105 | manual_object_[k]->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); 106 | } 107 | } 108 | 109 | void MultiProbMapDisplay::updateTopic() 110 | { 111 | unsubscribe(); 112 | subscribe(); 113 | clear(); 114 | } 115 | 116 | void MultiProbMapDisplay::clear() 117 | { 118 | setStatus( StatusProperty::Warn, "Message", "No map received" ); 119 | 120 | if( !loaded_ ) 121 | { 122 | return; 123 | } 124 | 125 | for (unsigned k = 0; k < manual_object_.size(); k++) 126 | { 127 | scene_manager_->destroyManualObject( manual_object_[k] ); 128 | std::string tex_name = texture_[k]->getName(); 129 | texture_[k].setNull(); 130 | Ogre::TextureManager::getSingleton().unload( tex_name ); 131 | } 132 | manual_object_.clear(); 133 | texture_.clear(); 134 | material_.clear(); 135 | 136 | loaded_ = false; 137 | } 138 | 139 | // *********************************************************************************************************************************** 140 | 141 | void MultiProbMapDisplay::update( float wall_dt, float ros_dt ) 142 | { 143 | { 144 | boost::mutex::scoped_lock lock(mutex_); 145 | current_map_ = updated_map_; 146 | } 147 | 148 | if (!new_map_) 149 | return; 150 | new_map_ = false; 151 | 152 | clear(); 153 | 154 | //ros::Time t[5]; 155 | //double dt[4] = {0,0,0,0}; 156 | for (unsigned int k = 0; k < current_map_->maps.size(); k++) 157 | { 158 | if (current_map_->maps[k].data.empty()) 159 | continue; 160 | setStatus( StatusProperty::Ok, "Message", "Map received" ); 161 | 162 | // Map info 163 | float resolution = current_map_->maps[k].info.resolution; 164 | int width = current_map_->maps[k].info.width; 165 | int height = current_map_->maps[k].info.height; 166 | 167 | // Load pixel 168 | //t[0] = ros::Time::now(); 169 | unsigned int pixels_size = width * height; 170 | unsigned char* pixels = new unsigned char[pixels_size]; 171 | memset(pixels, 255, pixels_size); 172 | unsigned int num_pixels_to_copy = pixels_size; 173 | if( pixels_size != current_map_->maps[k].data.size() ) 174 | if( current_map_->maps[k].data.size() < pixels_size ) 175 | num_pixels_to_copy = current_map_->maps[k].data.size(); 176 | for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) 177 | { 178 | unsigned char val; 179 | int8_t data = current_map_->maps[k].data[ pixel_index ]; 180 | if(data > 0) 181 | val = 255; 182 | else if(data < 0) 183 | val = 180; 184 | else 185 | val = 0; 186 | pixels[ pixel_index ] = val; 187 | } 188 | /* 189 | int pixels_size = current_map_->maps[k].data.size(); 190 | unsigned char* pixels = new unsigned char[pixels_size]; 191 | memcpy(pixels, ¤t_map_->maps[k].data[0], pixels_size); 192 | */ 193 | // Set texture 194 | //t[1] = ros::Time::now(); 195 | Ogre::DataStreamPtr pixel_stream; 196 | pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); 197 | static int tex_count = 0; 198 | std::stringstream ss1; 199 | ss1 << "MultiMapTexture" << tex_count++; 200 | Ogre::TexturePtr _texture_; 201 | //t[2] = ros::Time::now(); 202 | _texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss1.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 203 | pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, 0); 204 | //t[3] = ros::Time::now(); 205 | texture_.push_back(_texture_); 206 | delete [] pixels; 207 | setStatus( StatusProperty::Ok, "Map", "Map OK" ); 208 | //t[4] = ros::Time::now(); 209 | 210 | // Set material 211 | static int material_count = 0; 212 | std::stringstream ss0; 213 | ss0 << "MultiMapObjectMaterial" << material_count++; 214 | Ogre::MaterialPtr _material_; 215 | _material_ = Ogre::MaterialManager::getSingleton().create( ss0.str(), 216 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); 217 | _material_->setReceiveShadows(false); 218 | _material_->getTechnique(0)->setLightingEnabled(false); 219 | _material_->setDepthBias( -16.0f, 0.0f ); 220 | _material_->setCullingMode( Ogre::CULL_NONE ); 221 | _material_->setDepthWriteEnabled(false); 222 | material_.push_back(_material_); 223 | material_.back()->setSceneBlending( Ogre::SBT_TRANSPARENT_COLOUR ); 224 | material_.back()->setDepthWriteEnabled( false ); 225 | Ogre::Pass* pass = material_.back()->getTechnique(0)->getPass(0); 226 | Ogre::TextureUnitState* tex_unit = NULL; 227 | if (pass->getNumTextureUnitStates() > 0) 228 | tex_unit = pass->getTextureUnitState(0); 229 | else 230 | tex_unit = pass->createTextureUnitState(); 231 | tex_unit->setTextureName(texture_.back()->getName()); 232 | tex_unit->setTextureFiltering( Ogre::TFO_NONE ); 233 | 234 | // Set manual object 235 | static int map_count = 0; 236 | std::stringstream ss2; 237 | ss2 << "MultiMapObject" << map_count++; 238 | Ogre::ManualObject* _manual_object_ = scene_manager_->createManualObject( ss2.str() ); 239 | manual_object_.push_back(_manual_object_); 240 | scene_node_->attachObject( manual_object_.back() ); 241 | float yo = tf::getYaw(current_map_->origins[k].orientation); 242 | float co = cos(yo); 243 | float so = sin(yo); 244 | float dxo = current_map_->origins[k].position.x; 245 | float dyo = current_map_->origins[k].position.y; 246 | float ym = tf::getYaw(current_map_->maps[k].info.origin.orientation); 247 | float dxm = current_map_->maps[k].info.origin.position.x; 248 | float dym = current_map_->maps[k].info.origin.position.y; 249 | float yaw = yo + ym; 250 | float c = cos(yaw); 251 | float s = sin(yaw); 252 | float dx = co * dxm - so * dym + dxo; 253 | float dy = so * dxm + co * dym + dyo; 254 | float x = 0.0; 255 | float y = 0.0; 256 | manual_object_.back()->begin(material_.back()->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); 257 | { 258 | 259 | { 260 | 261 | x = c * 0.0 - s * 0.0 + dx; 262 | y = s * 0.0 + c * 0.0 + dy; 263 | manual_object_.back()->position( x, y, 0.0f ); 264 | manual_object_.back()->textureCoord(0.0f, 0.0f); 265 | manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); 266 | 267 | 268 | x = c * resolution*width - s * resolution*height + dx; 269 | y = s * resolution*width + c * resolution*height + dy; 270 | manual_object_.back()->position( x, y, 0.0f ); 271 | manual_object_.back()->textureCoord(1.0f, 1.0f); 272 | manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); 273 | 274 | 275 | x = c * 0.0 - s * resolution*height + dx; 276 | y = s * 0.0 + c * resolution*height + dy; 277 | manual_object_.back()->position( x, y, 0.0f ); 278 | manual_object_.back()->textureCoord(0.0f, 1.0f); 279 | manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); 280 | } 281 | 282 | 283 | { 284 | 285 | x = c * 0.0 - s * 0.0 + dx; 286 | y = s * 0.0 + c * 0.0 + dy; 287 | manual_object_.back()->position( x, y, 0.0f ); 288 | manual_object_.back()->textureCoord(0.0f, 0.0f); 289 | manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); 290 | 291 | 292 | x = c * resolution*width - s * 0.0 + dx; 293 | y = s * resolution*width + c * 0.0 + dy; 294 | manual_object_.back()->position( x, y, 0.0f ); 295 | manual_object_.back()->textureCoord(1.0f, 0.0f); 296 | manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); 297 | 298 | 299 | x = c * resolution*width - s * resolution*height + dx; 300 | y = s * resolution*width + c * resolution*height + dy; 301 | manual_object_.back()->position( x, y, 0.0f ); 302 | manual_object_.back()->textureCoord(1.0f, 1.0f); 303 | manual_object_.back()->normal( 0.0f, 0.0f, 1.0f ); 304 | } 305 | } 306 | manual_object_.back()->end(); 307 | if( draw_under_property_->getValue().toBool() ) 308 | manual_object_.back()->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); 309 | 310 | 311 | } 312 | loaded_ = true; 313 | context_->queueRender(); 314 | 315 | } 316 | 317 | // *********************************************************************************************************************************** 318 | 319 | void MultiProbMapDisplay::incomingMap(const multi_map_server::MultiOccupancyGrid::ConstPtr& msg) 320 | { 321 | updated_map_ = msg; 322 | boost::mutex::scoped_lock lock(mutex_); 323 | new_map_ = true; 324 | } 325 | 326 | void MultiProbMapDisplay::reset() 327 | { 328 | Display::reset(); 329 | clear(); 330 | updateTopic(); 331 | } 332 | 333 | } 334 | 335 | #include 336 | PLUGINLIB_EXPORT_CLASS( rviz::MultiProbMapDisplay, rviz::Display ) 337 | -------------------------------------------------------------------------------- /grid_path_searcher/src/Astar_searcher.cpp: -------------------------------------------------------------------------------- 1 | #include "Astar_searcher.h" 2 | 3 | using namespace std; 4 | using namespace Eigen; 5 | 6 | void AstarPathFinder::initGridMap(double _resolution, Vector3d global_xyz_l, Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id) 7 | { 8 | gl_xl = global_xyz_l(0); 9 | gl_yl = global_xyz_l(1); 10 | gl_zl = global_xyz_l(2); 11 | 12 | gl_xu = global_xyz_u(0); 13 | gl_yu = global_xyz_u(1); 14 | gl_zu = global_xyz_u(2); 15 | 16 | GLX_SIZE = max_x_id; 17 | GLY_SIZE = max_y_id; 18 | GLZ_SIZE = max_z_id; 19 | GLYZ_SIZE = GLY_SIZE * GLZ_SIZE; 20 | GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE; 21 | 22 | resolution = _resolution; 23 | inv_resolution = 1.0 / _resolution; 24 | 25 | data = new uint8_t[GLXYZ_SIZE]; 26 | 27 | memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t)); 28 | 29 | GridNodeMap = new GridNodePtr ** [GLX_SIZE]; 30 | for(int i = 0; i < GLX_SIZE; i++){ 31 | GridNodeMap[i] = new GridNodePtr * [GLY_SIZE]; 32 | for(int j = 0; j < GLY_SIZE; j++){ 33 | GridNodeMap[i][j] = new GridNodePtr [GLZ_SIZE]; 34 | for( int k = 0; k < GLZ_SIZE;k++){ 35 | Vector3i tmpIdx(i,j,k); 36 | Vector3d pos = gridIndex2coord(tmpIdx); 37 | GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos); 38 | } 39 | } 40 | } 41 | } 42 | 43 | void AstarPathFinder::resetGrid(GridNodePtr ptr) 44 | { 45 | ptr->id = 0; 46 | ptr->cameFrom = NULL; 47 | ptr->gScore = inf; 48 | ptr->fScore = inf; 49 | } 50 | 51 | void AstarPathFinder::resetUsedGrids() 52 | { 53 | for(int i=0; i < GLX_SIZE ; i++) 54 | for(int j=0; j < GLY_SIZE ; j++) 55 | for(int k=0; k < GLZ_SIZE ; k++) 56 | resetGrid(GridNodeMap[i][j][k]); 57 | } 58 | 59 | void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z) 60 | { 61 | if( coord_x < gl_xl || coord_y < gl_yl || coord_z < gl_zl || 62 | coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu ) 63 | return; 64 | 65 | int idx_x = static_cast( (coord_x - gl_xl) * inv_resolution); 66 | int idx_y = static_cast( (coord_y - gl_yl) * inv_resolution); 67 | int idx_z = static_cast( (coord_z - gl_zl) * inv_resolution); 68 | 69 | data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1; 70 | } 71 | 72 | 73 | vector AstarPathFinder::getVisitedNodes() 74 | { 75 | vector visited_nodes; 76 | for(int i = 0; i < GLX_SIZE; i++) 77 | for(int j = 0; j < GLY_SIZE; j++) 78 | for(int k = 0; k < GLZ_SIZE; k++){ 79 | if(GridNodeMap[i][j][k]->id == -1) 80 | visited_nodes.push_back(GridNodeMap[i][j][k]->coord); 81 | } 82 | 83 | ROS_WARN("visited_nodes size : %d", visited_nodes.size()); 84 | return visited_nodes; 85 | } 86 | 87 | Vector3d AstarPathFinder::gridIndex2coord(const Vector3i & index) 88 | { 89 | Vector3d pt; 90 | 91 | pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl; 92 | pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl; 93 | pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl; 94 | 95 | return pt; 96 | } 97 | 98 | 99 | Vector3i AstarPathFinder::coord2gridIndex(const Vector3d & pt) 100 | { 101 | Vector3i idx; 102 | idx << min( max( int( (pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1), 103 | min( max( int( (pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1), 104 | min( max( int( (pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1); 105 | 106 | return idx; 107 | } 108 | 109 | Eigen::Vector3d AstarPathFinder::coordRounding(const Eigen::Vector3d & coord) 110 | { 111 | return gridIndex2coord(coord2gridIndex(coord)); 112 | } 113 | 114 | inline bool AstarPathFinder::isOccupied(const Eigen::Vector3i & index) const 115 | { 116 | return isOccupied(index(0), index(1), index(2)); 117 | } 118 | 119 | inline bool AstarPathFinder::isFree(const Eigen::Vector3i & index) const 120 | { 121 | return isFree(index(0), index(1), index(2)); 122 | } 123 | 124 | inline bool AstarPathFinder::isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const 125 | { 126 | return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 127 | (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1)); 128 | } 129 | 130 | inline bool AstarPathFinder::isFree(const int & idx_x, const int & idx_y, const int & idx_z) const 131 | { 132 | return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && 133 | (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] < 1)); 134 | } 135 | 136 | 137 | inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector & neighborPtrSets, vector & edgeCostSets) 138 | { 139 | neighborPtrSets.clear(); 140 | edgeCostSets.clear(); 141 | 142 | if(currentPtr == nullptr) 143 | std::cout << "Error: Current pointer is null!" << endl; 144 | 145 | Eigen::Vector3i thisNode = currentPtr -> index; 146 | int this_x = thisNode[0]; 147 | int this_y = thisNode[1]; 148 | int this_z = thisNode[2]; 149 | auto this_coord = currentPtr -> coord; 150 | int n_x, n_y, n_z; 151 | double dist; 152 | 153 | GridNodePtr temp_ptr = nullptr; 154 | Eigen::Vector3d n_coord; 155 | 156 | for(int i = -1;i <= 1;++i ){ 157 | for(int j = -1;j <= 1;++j ){ 158 | for(int k = -1;k <= 1;++k){ 159 | if( i == 0 && j == 0 && k == 0) 160 | continue; 161 | 162 | n_x = this_x + i; 163 | n_y = this_y + j; 164 | n_z = this_z + k; 165 | 166 | if( (n_x < 0) || (n_x > (GLX_SIZE - 1)) || (n_y < 0) || (n_y > (GLY_SIZE - 1) ) || (n_z < 0) || (n_z > (GLZ_SIZE - 1))) 167 | continue; 168 | 169 | if(isOccupied(n_x, n_y, n_z)) 170 | continue; 171 | 172 | temp_ptr = GridNodeMap[n_x][n_y][n_z]; 173 | 174 | if(temp_ptr->id == -1) 175 | continue; 176 | 177 | n_coord = temp_ptr->coord; 178 | 179 | if(temp_ptr == currentPtr){ 180 | std::cout << "Error: temp_ptr == currentPtr)" << std::endl; 181 | } 182 | 183 | if( (std::abs(n_coord[0] - this_coord[0]) < 1e-6) and (std::abs(n_coord[1] - this_coord[1]) < 1e-6) and (std::abs(n_coord[2] - this_coord[2]) < 1e-6 )){ 184 | std::cout << "Error: Not expanding correctly!" << std::endl; 185 | std::cout << "n_coord:" << n_coord[0] << " "<coord; 208 | auto node2_coord = node2->coord; 209 | 210 | // Heuristics 1: Manhattan 211 | // h = std::abs(node1_coord(0) - node2_coord(0) ) + 212 | // std::abs(node1_coord(1) - node2_coord(1) ) + 213 | // std::abs(node1_coord(2) - node2_coord(2) ); 214 | 215 | //Heuristics 2: Euclidean 216 | // h = std::sqrt(std::pow((node1_coord(0) - node2_coord(0)), 2 ) + 217 | // std::pow((node1_coord(1) - node2_coord(1)), 2 ) + 218 | // std::pow((node1_coord(2) - node2_coord(2)), 2 )); 219 | 220 | // Heuristics 3: Diagnol distance 221 | double dx = std::abs(node1_coord(0) - node2_coord(0) ); 222 | double dy = std::abs(node1_coord(1) - node2_coord(1) ); 223 | double dz = std::abs(node1_coord(2) - node2_coord(2) ); 224 | double min_xyz = std::min({dx, dy, dz}); 225 | h = dx + dy + dz + (std::sqrt(3.0) -3) * min_xyz; 226 | 227 | return 0; //0 -> h DIjkstra -> Astar 228 | } 229 | 230 | //This is the better h(n) that will be applied in JPS_searcher.cpp 231 | double AstarPathFinder::getHeu2(GridNodePtr node1, GridNodePtr node2) 232 | { 233 | //f(n)=g(n)+w(n)*h(n) 234 | double h; //heuristic(n) 235 | double distance; //distance 236 | double w; //weight(n) 237 | 238 | w = 8.0; 239 | distance = calEuclideanDistance(node1, node2); 240 | // if(distance > 2) 241 | // { 242 | // w = 8.0; 243 | // } 244 | // else{ 245 | // w = 0.7; 246 | // } 247 | h = w * distance; 248 | 249 | return h; 250 | } 251 | 252 | //Heuristics 2: Euclidean Distance 253 | double AstarPathFinder::calEuclideanDistance(GridNodePtr node1, GridNodePtr node2) 254 | { 255 | double distance; 256 | auto node1_coord = node1->coord; 257 | auto node2_coord = node2->coord; 258 | 259 | distance = std::sqrt(std::pow((node1_coord(0) - node2_coord(0)), 2 ) + 260 | std::pow((node1_coord(1) - node2_coord(1)), 2 ) + 261 | std::pow((node1_coord(2) - node2_coord(2)), 2 )); 262 | return distance; 263 | } 264 | 265 | void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt) 266 | { 267 | ros::Time time_1 = ros::Time::now(); 268 | 269 | Vector3i start_idx = coord2gridIndex(start_pt); 270 | Vector3i end_idx = coord2gridIndex(end_pt); 271 | goalIdx = end_idx; 272 | 273 | start_pt = gridIndex2coord(start_idx); 274 | end_pt = gridIndex2coord(end_idx); 275 | 276 | GridNodePtr startPtr = new GridNode(start_idx, start_pt); 277 | GridNodePtr endPtr = new GridNode(end_idx, end_pt); 278 | 279 | openSet.clear(); 280 | GridNodePtr currentPtr = NULL; 281 | GridNodePtr neighborPtr = NULL; 282 | 283 | 284 | startPtr -> gScore = 0; 285 | startPtr -> fScore = getHeu(startPtr,endPtr); 286 | startPtr -> id = 1; 287 | startPtr -> coord = start_pt; 288 | openSet.insert( make_pair(startPtr -> fScore, startPtr) ); 289 | 290 | GridNodeMap[start_idx[0]][start_idx[1]][start_idx[2]] -> id = 1; 291 | 292 | vector neighborPtrSets; 293 | vector edgeCostSets; 294 | Eigen::Vector3i current_idx; 295 | 296 | while ( !openSet.empty() ){ 297 | 298 | int x = openSet.begin()->second->index(0); 299 | int y = openSet.begin()->second->index(1); 300 | int z = openSet.begin()->second->index(2); 301 | openSet.erase(openSet.begin()); 302 | currentPtr = GridNodeMap[x][y][z]; 303 | 304 | if(currentPtr->id == -1) 305 | continue; 306 | 307 | currentPtr->id = -1; 308 | 309 | 310 | if( currentPtr->index == goalIdx ) 311 | { 312 | ros::Time time_2 = ros::Time::now(); 313 | terminatePtr = currentPtr; 314 | ROS_WARN("[A*_white] succeed ! Time consumed : %f ms, path cost : %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution ); 315 | return; 316 | } 317 | 318 | 319 | AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets); 320 | 321 | for(int i = 0; i < (int)neighborPtrSets.size(); i++){ 322 | 323 | neighborPtr = neighborPtrSets[i]; 324 | if(neighborPtr -> id == 0){ 325 | 326 | neighborPtr->gScore = currentPtr->gScore + edgeCostSets[i]; 327 | neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr,endPtr); 328 | neighborPtr->cameFrom = currentPtr; 329 | 330 | openSet.insert(make_pair(neighborPtr -> fScore, neighborPtr)); 331 | neighborPtr -> id = 1; 332 | continue; 333 | } 334 | else if(neighborPtr -> id == 1){ 335 | if( neighborPtr->gScore > (currentPtr->gScore+edgeCostSets[i])) 336 | { 337 | neighborPtr -> gScore = currentPtr -> gScore + edgeCostSets[i]; 338 | neighborPtr -> fScore = neighborPtr -> gScore + getHeu(neighborPtr,endPtr); 339 | neighborPtr -> cameFrom = currentPtr; 340 | } 341 | 342 | continue; 343 | } 344 | else{ 345 | continue; 346 | } 347 | } 348 | } 349 | 350 | ros::Time time_2 = ros::Time::now(); 351 | if((time_2 - time_1).toSec() > 0.1) 352 | ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec() ); 353 | } 354 | 355 | 356 | vector AstarPathFinder::getPath() 357 | { 358 | vector path; 359 | vector gridPath; 360 | 361 | auto ptr = terminatePtr; 362 | while(ptr -> cameFrom != NULL){ 363 | gridPath.push_back(ptr); 364 | ptr = ptr->cameFrom; 365 | } 366 | 367 | for (auto ptr: gridPath) 368 | path.push_back(ptr->coord); 369 | 370 | reverse(path.begin(),path.end()); 371 | return path; 372 | } 373 | -------------------------------------------------------------------------------- /grid_path_searcher/src/JPS_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "JPS_utils.h" 2 | 3 | constexpr int JPS3DNeib::nsz[4][2]; 4 | JPS3DNeib::JPS3DNeib() 5 | { 6 | int id = 0; 7 | for(int dz = -1; dz <= 1; ++ dz) 8 | { 9 | for(int dy = -1; dy <= 1; ++ dy) 10 | { 11 | for(int dx = -1; dx <= 1; ++ dx) 12 | { 13 | int norm1 = abs(dx) + abs(dy) + abs(dz); //0~3 14 | 15 | for(int dev = 0; dev < nsz[norm1][0]; ++ dev) //norm1 = 0,dev = 0~25 16 | Neib(dx,dy,dz,norm1,dev, ns[id][0][dev], ns[id][1][dev], ns[id][2][dev]); 17 | 18 | for(int dev = 0; dev < nsz[norm1][1]; ++ dev) 19 | { 20 | FNeib(dx,dy,dz,norm1,dev, 21 | f1[id][0][dev],f1[id][1][dev], f1[id][2][dev], 22 | f2[id][0][dev],f2[id][1][dev], f2[id][2][dev]); 23 | } 24 | 25 | id ++; 26 | } 27 | } 28 | } 29 | } 30 | 31 | 32 | void JPS3DNeib::Neib(int dx, int dy, int dz, int norm1, int dev, 33 | int& tx, int& ty, int& tz) 34 | { 35 | switch(norm1) 36 | { 37 | case 0: // 0 38 | switch(dev) 39 | { 40 | case 0: tx=1; ty=0; tz=0; return; 41 | case 1: tx=-1; ty=0; tz=0; return; 42 | case 2: tx=0; ty=1; tz=0; return; 43 | case 3: tx=1; ty=1; tz=0; return; 44 | case 4: tx=-1; ty=1; tz=0; return; 45 | case 5: tx=0; ty=-1; tz=0; return; 46 | case 6: tx=1; ty=-1; tz=0; return; 47 | case 7: tx=-1; ty=-1; tz=0; return; 48 | case 8: tx=0; ty=0; tz=1; return; 49 | case 9: tx=1; ty=0; tz=1; return; 50 | case 10: tx=-1; ty=0; tz=1; return; 51 | case 11: tx=0; ty=1; tz=1; return; 52 | case 12: tx=1; ty=1; tz=1; return; 53 | case 13: tx=-1; ty=1; tz=1; return; 54 | case 14: tx=0; ty=-1; tz=1; return; 55 | case 15: tx=1; ty=-1; tz=1; return; 56 | case 16: tx=-1; ty=-1; tz=1; return; 57 | case 17: tx=0; ty=0; tz=-1; return; 58 | case 18: tx=1; ty=0; tz=-1; return; 59 | case 19: tx=-1; ty=0; tz=-1; return; 60 | case 20: tx=0; ty=1; tz=-1; return; 61 | case 21: tx=1; ty=1; tz=-1; return; 62 | case 22: tx=-1; ty=1; tz=-1; return; 63 | case 23: tx=0; ty=-1; tz=-1; return; 64 | case 24: tx=1; ty=-1; tz=-1; return; 65 | case 25: tx=-1; ty=-1; tz=-1; return; 66 | } 67 | case 1: // 1 68 | tx = dx; ty = dy; tz = dz; return; 69 | case 2: // sqrt2 70 | switch(dev) 71 | { 72 | case 0: 73 | if(dz == 0) 74 | { 75 | tx = 0; ty = dy; tz = 0; return; 76 | } 77 | else 78 | { 79 | tx = 0; ty = 0; tz = dz; return; 80 | } 81 | case 1: 82 | if(dx == 0) 83 | { 84 | tx = 0; ty = dy; tz = 0; return; 85 | } 86 | else 87 | { 88 | tx = dx; ty = 0; tz = 0; return; 89 | } 90 | case 2: 91 | tx = dx; ty = dy; tz = dz; return; 92 | } 93 | case 3: //sqrt3 94 | switch(dev) 95 | { 96 | case 0: tx = dx; ty = 0; tz = 0; return; 97 | case 1: tx = 0; ty = dy; tz = 0; return; 98 | case 2: tx = 0; ty = 0; tz = dz; return; 99 | case 3: tx = dx; ty = dy; tz = 0; return; 100 | case 4: tx = dx; ty = 0; tz = dz; return; 101 | case 5: tx = 0; ty = dy; tz = dz; return; 102 | case 6: tx = dx; ty = dy; tz = dz; return; 103 | } 104 | } 105 | } 106 | 107 | void JPS3DNeib::FNeib( int dx, int dy, int dz, int norm1, int dev, 108 | int& fx, int& fy, int& fz, 109 | int& nx, int& ny, int& nz) 110 | { 111 | switch(norm1) 112 | { 113 | case 1: 114 | switch(dev) 115 | { 116 | case 0: fx= 0; fy= 1; fz = 0; break; 117 | case 1: fx= 0; fy=-1; fz = 0; break; 118 | case 2: fx= 1; fy= 0; fz = 0; break; 119 | case 3: fx= 1; fy= 1; fz = 0; break; 120 | case 4: fx= 1; fy=-1; fz = 0; break; 121 | case 5: fx=-1; fy= 0; fz = 0; break; 122 | case 6: fx=-1; fy= 1; fz = 0; break; 123 | case 7: fx=-1; fy=-1; fz = 0; break; 124 | } 125 | nx = fx; ny = fy; nz = dz; 126 | // switch order if different direction 127 | if(dx != 0) 128 | { 129 | fz = fx; fx = 0; 130 | nz = fz; nx = dx; 131 | } 132 | 133 | if(dy != 0) 134 | { 135 | fz = fy; fy = 0; 136 | nz = fz; ny = dy; 137 | } 138 | return; 139 | case 2: 140 | if(dx == 0) 141 | { 142 | switch(dev) 143 | { 144 | case 0: 145 | fx = 0; fy = 0; fz = -dz; 146 | nx = 0; ny = dy; nz = -dz; 147 | return; 148 | case 1: 149 | fx = 0; fy = -dy; fz = 0; 150 | nx = 0; ny = -dy; nz = dz; 151 | return; 152 | case 2: 153 | fx = 1; fy = 0; fz = 0; 154 | nx = 1; ny = dy; nz = dz; 155 | return; 156 | case 3: 157 | fx = -1; fy = 0; fz = 0; 158 | nx = -1; ny = dy; nz = dz; 159 | return; 160 | case 4: 161 | fx = 1; fy = 0; fz = -dz; 162 | nx = 1; ny = dy; nz = -dz; 163 | return; 164 | case 5: 165 | fx = 1; fy = -dy; fz = 0; 166 | nx = 1; ny = -dy; nz = dz; 167 | return; 168 | case 6: 169 | fx = -1; fy = 0; fz = -dz; 170 | nx = -1; ny = dy; nz = -dz; 171 | return; 172 | case 7: 173 | fx = -1; fy = -dy; fz = 0; 174 | nx = -1; ny = -dy; nz = dz; 175 | return; 176 | // Extras 177 | case 8: 178 | fx = 1; fy = 0; fz = 0; 179 | nx = 1; ny = dy; nz = 0; 180 | return; 181 | case 9: 182 | fx = 1; fy = 0; fz = 0; 183 | nx = 1; ny = 0; nz = dz; 184 | return; 185 | case 10: 186 | fx = -1; fy = 0; fz = 0; 187 | nx = -1; ny = dy; nz = 0; 188 | return; 189 | case 11: 190 | fx = -1; fy = 0; fz = 0; 191 | nx = -1; ny = 0; nz = dz; 192 | return; 193 | } 194 | } 195 | else if(dy == 0) 196 | { 197 | switch(dev) 198 | { 199 | case 0: 200 | fx = 0; fy = 0; fz = -dz; 201 | nx = dx; ny = 0; nz = -dz; 202 | return; 203 | case 1: 204 | fx = -dx; fy = 0; fz = 0; 205 | nx = -dx; ny = 0; nz = dz; 206 | return; 207 | case 2: 208 | fx = 0; fy = 1; fz = 0; 209 | nx = dx; ny = 1; nz = dz; 210 | return; 211 | case 3: 212 | fx = 0; fy = -1; fz = 0; 213 | nx = dx; ny = -1;nz = dz; 214 | return; 215 | case 4: 216 | fx = 0; fy = 1; fz = -dz; 217 | nx = dx; ny = 1; nz = -dz; 218 | return; 219 | case 5: 220 | fx = -dx; fy = 1; fz = 0; 221 | nx = -dx; ny = 1; nz = dz; 222 | return; 223 | case 6: 224 | fx = 0; fy = -1; fz = -dz; 225 | nx = dx; ny = -1; nz = -dz; 226 | return; 227 | case 7: 228 | fx = -dx; fy = -1; fz = 0; 229 | nx = -dx; ny = -1; nz = dz; 230 | return; 231 | // Extras 232 | case 8: 233 | fx = 0; fy = 1; fz = 0; 234 | nx = dx; ny = 1; nz = 0; 235 | return; 236 | case 9: 237 | fx = 0; fy = 1; fz = 0; 238 | nx = 0; ny = 1; nz = dz; 239 | return; 240 | case 10: 241 | fx = 0; fy = -1; fz = 0; 242 | nx = dx; ny = -1; nz = 0; 243 | return; 244 | case 11: 245 | fx = 0; fy = -1; fz = 0; 246 | nx = 0; ny = -1; nz = dz; 247 | return; 248 | } 249 | } 250 | else 251 | {// dz==0 252 | switch(dev) 253 | { 254 | case 0: 255 | fx = 0; fy = -dy; fz = 0; 256 | nx = dx; ny = -dy; nz = 0; 257 | return; 258 | case 1: 259 | fx = -dx; fy = 0; fz = 0; 260 | nx = -dx; ny = dy; nz = 0; 261 | return; 262 | case 2: 263 | fx = 0; fy = 0; fz = 1; 264 | nx = dx; ny = dy; nz = 1; 265 | return; 266 | case 3: 267 | fx = 0; fy = 0; fz = -1; 268 | nx = dx; ny = dy; nz = -1; 269 | return; 270 | case 4: 271 | fx = 0; fy = -dy; fz = 1; 272 | nx = dx; ny = -dy; nz = 1; 273 | return; 274 | case 5: 275 | fx = -dx; fy = 0; fz = 1; 276 | nx = -dx; ny = dy; nz = 1; 277 | return; 278 | case 6: 279 | fx = 0; fy = -dy; fz = -1; 280 | nx = dx; ny = -dy; nz = -1; 281 | return; 282 | case 7: 283 | fx = -dx; fy = 0; fz = -1; 284 | nx = -dx; ny = dy; nz = -1; 285 | return; 286 | // Extras 287 | case 8: 288 | fx = 0; fy = 0; fz = 1; 289 | nx = dx; ny = 0; nz = 1; 290 | return; 291 | case 9: 292 | fx = 0; fy = 0; fz = 1; 293 | nx = 0; ny = dy; nz = 1; 294 | return; 295 | case 10: 296 | fx = 0; fy = 0; fz = -1; 297 | nx = dx; ny = 0; nz = -1; 298 | return; 299 | case 11: 300 | fx = 0; fy = 0; fz = -1; 301 | nx = 0; ny = dy; nz = -1; 302 | return; 303 | } 304 | } 305 | case 3: 306 | switch(dev) 307 | { 308 | case 0: 309 | fx = -dx; fy = 0; fz = 0; 310 | nx = -dx; ny = dy; nz = dz; 311 | return; 312 | case 1: 313 | fx = 0; fy = -dy; fz = 0; 314 | nx = dx; ny = -dy; nz = dz; 315 | return; 316 | case 2: 317 | fx = 0; fy = 0; fz = -dz; 318 | nx = dx; ny = dy; nz = -dz; 319 | return; 320 | // Need to check up to here for forced! 321 | case 3: 322 | fx = 0; fy = -dy; fz = -dz; 323 | nx = dx; ny = -dy; nz = -dz; 324 | return; 325 | case 4: 326 | fx = -dx; fy = 0; fz = -dz; 327 | nx = -dx; ny = dy; nz = -dz; 328 | return; 329 | case 5: 330 | fx = -dx; fy = -dy; fz = 0; 331 | nx = -dx; ny = -dy; nz = dz; 332 | return; 333 | // Extras 334 | case 6: 335 | fx = -dx; fy = 0; fz = 0; 336 | nx = -dx; ny = 0; nz = dz; 337 | return; 338 | case 7: 339 | fx = -dx; fy = 0; fz = 0; 340 | nx = -dx; ny = dy; nz = 0; 341 | return; 342 | case 8: 343 | fx = 0; fy = -dy; fz = 0; 344 | nx = 0; ny = -dy; nz = dz; 345 | return; 346 | case 9: 347 | fx = 0; fy = -dy; fz = 0; 348 | nx = dx; ny = -dy; nz = 0; 349 | return; 350 | case 10: 351 | fx = 0; fy = 0; fz = -dz; 352 | nx = 0; ny = dy; nz = -dz; 353 | return; 354 | case 11: 355 | fx = 0; fy = 0; fz = -dz; 356 | nx = dx; ny = 0; nz = -dz; 357 | return; 358 | } 359 | } 360 | } -------------------------------------------------------------------------------- /rviz_plugins/src/aerialmap_display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include "rviz/frame_manager.h" 14 | #include "rviz/ogre_helpers/grid.h" 15 | #include "rviz/properties/float_property.h" 16 | #include "rviz/properties/int_property.h" 17 | #include "rviz/properties/property.h" 18 | #include "rviz/properties/quaternion_property.h" 19 | #include "rviz/properties/ros_topic_property.h" 20 | #include "rviz/properties/vector_property.h" 21 | #include "rviz/validate_floats.h" 22 | #include "rviz/display_context.h" 23 | 24 | #include "aerialmap_display.h" 25 | 26 | namespace rviz 27 | { 28 | 29 | AerialMapDisplay::AerialMapDisplay() 30 | : Display() 31 | , manual_object_( NULL ) 32 | //, material_( 0 ) 33 | , loaded_( false ) 34 | , resolution_( 0.0f ) 35 | , width_( 0 ) 36 | , height_( 0 ) 37 | , position_(Ogre::Vector3::ZERO) 38 | , orientation_(Ogre::Quaternion::IDENTITY) 39 | , new_map_(false) 40 | { 41 | topic_property_ = new RosTopicProperty( "Topic", "", 42 | QString::fromStdString( ros::message_traits::datatype() ), 43 | "nav_msgs::OccupancyGrid topic to subscribe to.", 44 | this, SLOT( updateTopic() )); 45 | 46 | alpha_property_ = new FloatProperty( "Alpha", 0.7, 47 | "Amount of transparency to apply to the map.", 48 | this, SLOT( updateAlpha() )); 49 | alpha_property_->setMin( 0 ); 50 | alpha_property_->setMax( 1 ); 51 | 52 | draw_under_property_ = new Property( "Draw Behind", false, 53 | "Rendering option, controls whether or not the map is always" 54 | " drawn behind everything else.", 55 | this, SLOT( updateDrawUnder() )); 56 | 57 | resolution_property_ = new FloatProperty( "Resolution", 0, 58 | "Resolution of the map. (not editable)", this ); 59 | resolution_property_->setReadOnly( true ); 60 | 61 | width_property_ = new IntProperty( "Width", 0, 62 | "Width of the map, in meters. (not editable)", this ); 63 | width_property_->setReadOnly( true ); 64 | 65 | height_property_ = new IntProperty( "Height", 0, 66 | "Height of the map, in meters. (not editable)", this ); 67 | height_property_->setReadOnly( true ); 68 | 69 | position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, 70 | "Position of the bottom left corner of the map, in meters. (not editable)", 71 | this ); 72 | position_property_->setReadOnly( true ); 73 | 74 | orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, 75 | "Orientation of the map. (not editable)", 76 | this ); 77 | orientation_property_->setReadOnly( true ); 78 | } 79 | 80 | AerialMapDisplay::~AerialMapDisplay() 81 | { 82 | unsubscribe(); 83 | clear(); 84 | } 85 | 86 | void AerialMapDisplay::onInitialize() 87 | { 88 | static int count = 0; 89 | std::stringstream ss; 90 | ss << "AerialMapObjectMaterial" << count++; 91 | material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), 92 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); 93 | material_->setReceiveShadows(false); 94 | material_->getTechnique(0)->setLightingEnabled(false); 95 | material_->setDepthBias( -16.0f, 0.0f ); 96 | material_->setCullingMode( Ogre::CULL_NONE ); 97 | material_->setDepthWriteEnabled(false); 98 | 99 | updateAlpha(); 100 | } 101 | 102 | void AerialMapDisplay::onEnable() 103 | { 104 | subscribe(); 105 | } 106 | 107 | void AerialMapDisplay::onDisable() 108 | { 109 | unsubscribe(); 110 | clear(); 111 | } 112 | 113 | void AerialMapDisplay::subscribe() 114 | { 115 | if ( !isEnabled() ) 116 | { 117 | return; 118 | } 119 | 120 | if( !topic_property_->getTopic().isEmpty() ) 121 | { 122 | try 123 | { 124 | map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &AerialMapDisplay::incomingAerialMap, this ); 125 | setStatus( StatusProperty::Ok, "Topic", "OK" ); 126 | } 127 | catch( ros::Exception& e ) 128 | { 129 | setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); 130 | } 131 | } 132 | } 133 | 134 | void AerialMapDisplay::unsubscribe() 135 | { 136 | map_sub_.shutdown(); 137 | } 138 | 139 | void AerialMapDisplay::updateAlpha() 140 | { 141 | float alpha = alpha_property_->getFloat(); 142 | 143 | Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 ); 144 | Ogre::TextureUnitState* tex_unit = NULL; 145 | if( pass->getNumTextureUnitStates() > 0 ) 146 | { 147 | tex_unit = pass->getTextureUnitState( 0 ); 148 | } 149 | else 150 | { 151 | tex_unit = pass->createTextureUnitState(); 152 | } 153 | 154 | tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha ); 155 | 156 | if( alpha < 0.9998 ) 157 | { 158 | material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); 159 | material_->setDepthWriteEnabled( false ); 160 | } 161 | else 162 | { 163 | material_->setSceneBlending( Ogre::SBT_REPLACE ); 164 | material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() ); 165 | } 166 | } 167 | 168 | void AerialMapDisplay::updateDrawUnder() 169 | { 170 | bool draw_under = draw_under_property_->getValue().toBool(); 171 | 172 | if( alpha_property_->getFloat() >= 0.9998 ) 173 | { 174 | material_->setDepthWriteEnabled( !draw_under ); 175 | } 176 | 177 | if( manual_object_ ) 178 | { 179 | if( draw_under ) 180 | { 181 | manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); 182 | } 183 | else 184 | { 185 | manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); 186 | } 187 | } 188 | } 189 | 190 | void AerialMapDisplay::updateTopic() 191 | { 192 | unsubscribe(); 193 | subscribe(); 194 | clear(); 195 | } 196 | 197 | void AerialMapDisplay::clear() 198 | { 199 | setStatus( StatusProperty::Warn, "Message", "No map received" ); 200 | 201 | if( !loaded_ ) 202 | { 203 | return; 204 | } 205 | 206 | scene_manager_->destroyManualObject( manual_object_ ); 207 | manual_object_ = NULL; 208 | 209 | std::string tex_name = texture_->getName(); 210 | texture_.setNull(); 211 | Ogre::TextureManager::getSingleton().unload( tex_name ); 212 | 213 | loaded_ = false; 214 | } 215 | /* 216 | bool validateFloats(const nav_msgs::OccupancyGrid& msg) 217 | { 218 | bool valid = true; 219 | valid = valid && validateFloats( msg.info.resolution ); 220 | valid = valid && validateFloats( msg.info.origin ); 221 | return valid; 222 | } 223 | */ 224 | void AerialMapDisplay::update( float wall_dt, float ros_dt ) 225 | { 226 | { 227 | boost::mutex::scoped_lock lock(mutex_); 228 | 229 | current_map_ = updated_map_; 230 | } 231 | 232 | if (!current_map_ || !new_map_) 233 | { 234 | return; 235 | } 236 | 237 | if (current_map_->data.empty()) 238 | { 239 | return; 240 | } 241 | 242 | new_map_ = false; 243 | 244 | if( current_map_->info.width * current_map_->info.height == 0 ) 245 | { 246 | std::stringstream ss; 247 | ss << "AerialMap is zero-sized (" << current_map_->info.width << "x" << current_map_->info.height << ")"; 248 | setStatus( StatusProperty::Error, "AerialMap", QString::fromStdString( ss.str() )); 249 | return; 250 | } 251 | 252 | clear(); 253 | 254 | setStatus( StatusProperty::Ok, "Message", "AerialMap received" ); 255 | 256 | ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n", 257 | current_map_->info.width, 258 | current_map_->info.height, 259 | current_map_->info.resolution ); 260 | 261 | float resolution = current_map_->info.resolution; 262 | 263 | int width = current_map_->info.width; 264 | int height = current_map_->info.height; 265 | 266 | 267 | Ogre::Vector3 position( current_map_->info.origin.position.x, 268 | current_map_->info.origin.position.y, 269 | current_map_->info.origin.position.z ); 270 | Ogre::Quaternion orientation( current_map_->info.origin.orientation.w, 271 | current_map_->info.origin.orientation.x, 272 | current_map_->info.origin.orientation.y, 273 | current_map_->info.origin.orientation.z ); 274 | frame_ = current_map_->header.frame_id; 275 | if (frame_.empty()) 276 | { 277 | frame_ = "/map"; 278 | } 279 | 280 | // Expand it to be RGB data 281 | unsigned int pixels_size = width * height * 3; 282 | unsigned char* pixels = new unsigned char[pixels_size]; 283 | memset(pixels, 255, pixels_size); 284 | 285 | bool map_status_set = false; 286 | unsigned int num_pixels_to_copy = pixels_size; 287 | if( pixels_size != current_map_->data.size() ) 288 | { 289 | std::stringstream ss; 290 | ss << "Data size doesn't match width*height: width = " << width 291 | << ", height = " << height << ", data size = " << current_map_->data.size(); 292 | setStatus( StatusProperty::Error, "AerialMap", QString::fromStdString( ss.str() )); 293 | map_status_set = true; 294 | 295 | // Keep going, but don't read past the end of the data. 296 | if( current_map_->data.size() < pixels_size ) 297 | { 298 | num_pixels_to_copy = current_map_->data.size(); 299 | } 300 | } 301 | 302 | // TODO: a fragment shader could do this on the video card, and 303 | // would allow a non-grayscale color to mark the out-of-range 304 | // values. 305 | for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) 306 | { 307 | /* 308 | unsigned char val; 309 | int8_t data = current_map_->data[ pixel_index ]; 310 | if(data > 0) 311 | val = 0; 312 | else if(data < 0) 313 | val = 255; 314 | else 315 | val = 127; 316 | pixels[ pixel_index ] = val; 317 | */ 318 | pixels[ pixel_index ] = current_map_->data[ pixel_index ]; 319 | } 320 | 321 | Ogre::DataStreamPtr pixel_stream; 322 | pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); 323 | static int tex_count = 0; 324 | std::stringstream ss; 325 | ss << "AerialMapTexture" << tex_count++; 326 | try 327 | { 328 | texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 329 | pixel_stream, width, height, Ogre::PF_R8G8B8, Ogre::TEX_TYPE_2D, 330 | 0); 331 | 332 | if( !map_status_set ) 333 | { 334 | setStatus( StatusProperty::Ok, "AerialMap", "AerialMap OK" ); 335 | } 336 | } 337 | catch(Ogre::RenderingAPIException&) 338 | { 339 | Ogre::Image image; 340 | pixel_stream->seek(0); 341 | float fwidth = width; 342 | float fheight = height; 343 | if( width > height ) 344 | { 345 | float aspect = fheight / fwidth; 346 | fwidth = 2048; 347 | fheight = fwidth * aspect; 348 | } 349 | else 350 | { 351 | float aspect = fwidth / fheight; 352 | fheight = 2048; 353 | fwidth = fheight * aspect; 354 | } 355 | 356 | { 357 | std::stringstream ss; 358 | ss << "AerialMap is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]"; 359 | setStatus(StatusProperty::Ok, "AerialMap", QString::fromStdString( ss.str() )); 360 | } 361 | 362 | ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048. Downsampling to [%d x %d]...", (int)fwidth, (int)fheight); 363 | 364 | image.loadRawData(pixel_stream, width, height, Ogre::PF_R8G8B8); 365 | image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); 366 | ss << "Downsampled"; 367 | texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); 368 | } 369 | 370 | delete [] pixels; 371 | 372 | Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); 373 | Ogre::TextureUnitState* tex_unit = NULL; 374 | if (pass->getNumTextureUnitStates() > 0) 375 | { 376 | tex_unit = pass->getTextureUnitState(0); 377 | } 378 | else 379 | { 380 | tex_unit = pass->createTextureUnitState(); 381 | } 382 | 383 | tex_unit->setTextureName(texture_->getName()); 384 | tex_unit->setTextureFiltering( Ogre::TFO_NONE ); 385 | 386 | static int map_count = 0; 387 | std::stringstream ss2; 388 | ss2 << "AerialMapObject" << map_count++; 389 | manual_object_ = scene_manager_->createManualObject( ss2.str() ); 390 | scene_node_->attachObject( manual_object_ ); 391 | 392 | manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); 393 | { 394 | 395 | { 396 | manual_object_->position( 0.0f, 0.0f, 0.0f ); 397 | manual_object_->textureCoord(0.0f, 0.0f); 398 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 399 | 400 | manual_object_->position( resolution*width, resolution*height, 0.0f ); 401 | manual_object_->textureCoord(1.0f, 1.0f); 402 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 403 | 404 | manual_object_->position( 0.0f, resolution*height, 0.0f ); 405 | manual_object_->textureCoord(0.0f, 1.0f); 406 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 407 | } 408 | 409 | { 410 | manual_object_->position( 0.0f, 0.0f, 0.0f ); 411 | manual_object_->textureCoord(0.0f, 0.0f); 412 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 413 | 414 | manual_object_->position( resolution*width, 0.0f, 0.0f ); 415 | manual_object_->textureCoord(1.0f, 0.0f); 416 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 417 | 418 | manual_object_->position( resolution*width, resolution*height, 0.0f ); 419 | manual_object_->textureCoord(1.0f, 1.0f); 420 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 421 | } 422 | } 423 | manual_object_->end(); 424 | 425 | if( draw_under_property_->getValue().toBool() ) 426 | { 427 | manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); 428 | } 429 | 430 | resolution_property_->setValue( resolution ); 431 | width_property_->setValue( width ); 432 | height_property_->setValue( height ); 433 | position_property_->setVector( position ); 434 | orientation_property_->setQuaternion( orientation ); 435 | 436 | transformAerialMap(); 437 | 438 | loaded_ = true; 439 | 440 | context_->queueRender(); 441 | } 442 | 443 | void AerialMapDisplay::incomingAerialMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) 444 | { 445 | 446 | updated_map_ = msg; 447 | boost::mutex::scoped_lock lock(mutex_); 448 | new_map_ = true; 449 | } 450 | 451 | 452 | 453 | void AerialMapDisplay::transformAerialMap() 454 | { 455 | if (!current_map_) 456 | { 457 | return; 458 | } 459 | 460 | Ogre::Vector3 position; 461 | Ogre::Quaternion orientation; 462 | if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_->info.origin, position, orientation)) 463 | { 464 | ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'", 465 | qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ )); 466 | 467 | setStatus( StatusProperty::Error, "Transform", 468 | "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" ); 469 | } 470 | else 471 | { 472 | setStatus(StatusProperty::Ok, "Transform", "Transform OK"); 473 | } 474 | 475 | scene_node_->setPosition( position ); 476 | scene_node_->setOrientation( orientation ); 477 | } 478 | 479 | void AerialMapDisplay::fixedFrameChanged() 480 | { 481 | transformAerialMap(); 482 | } 483 | 484 | void AerialMapDisplay::reset() 485 | { 486 | Display::reset(); 487 | 488 | clear(); 489 | 490 | updateTopic(); 491 | } 492 | 493 | } 494 | 495 | #include 496 | PLUGINLIB_EXPORT_CLASS( rviz::AerialMapDisplay, rviz::Display ) 497 | -------------------------------------------------------------------------------- /rviz_plugins/src/probmap_display.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include "rviz/frame_manager.h" 15 | #include "rviz/ogre_helpers/grid.h" 16 | #include "rviz/properties/float_property.h" 17 | #include "rviz/properties/int_property.h" 18 | #include "rviz/properties/property.h" 19 | #include "rviz/properties/quaternion_property.h" 20 | #include "rviz/properties/ros_topic_property.h" 21 | #include "rviz/properties/vector_property.h" 22 | #include "rviz/validate_floats.h" 23 | #include "rviz/display_context.h" 24 | 25 | #include "probmap_display.h" 26 | 27 | namespace rviz 28 | { 29 | 30 | ProbMapDisplay::ProbMapDisplay() 31 | : Display() 32 | , manual_object_( NULL ) 33 | 34 | , loaded_( false ) 35 | , resolution_( 0.0f ) 36 | , width_( 0 ) 37 | , height_( 0 ) 38 | , position_(Ogre::Vector3::ZERO) 39 | , orientation_(Ogre::Quaternion::IDENTITY) 40 | , new_map_(false) 41 | { 42 | topic_property_ = new RosTopicProperty( "Topic", "", 43 | QString::fromStdString( ros::message_traits::datatype() ), 44 | "nav_msgs::OccupancyGrid topic to subscribe to.", 45 | this, SLOT( updateTopic() )); 46 | 47 | alpha_property_ = new FloatProperty( "Alpha", 0.7, 48 | "Amount of transparency to apply to the map.", 49 | this, SLOT( updateAlpha() )); 50 | alpha_property_->setMin( 0 ); 51 | alpha_property_->setMax( 1 ); 52 | 53 | draw_under_property_ = new Property( "Draw Behind", false, 54 | "Rendering option, controls whether or not the map is always" 55 | " drawn behind everything else.", 56 | this, SLOT( updateDrawUnder() )); 57 | 58 | resolution_property_ = new FloatProperty( "Resolution", 0, 59 | "Resolution of the map. (not editable)", this ); 60 | resolution_property_->setReadOnly( true ); 61 | 62 | width_property_ = new IntProperty( "Width", 0, 63 | "Width of the map, in meters. (not editable)", this ); 64 | width_property_->setReadOnly( true ); 65 | 66 | height_property_ = new IntProperty( "Height", 0, 67 | "Height of the map, in meters. (not editable)", this ); 68 | height_property_->setReadOnly( true ); 69 | 70 | position_property_ = new VectorProperty( "Position", Ogre::Vector3::ZERO, 71 | "Position of the bottom left corner of the map, in meters. (not editable)", 72 | this ); 73 | position_property_->setReadOnly( true ); 74 | 75 | orientation_property_ = new QuaternionProperty( "Orientation", Ogre::Quaternion::IDENTITY, 76 | "Orientation of the map. (not editable)", 77 | this ); 78 | orientation_property_->setReadOnly( true ); 79 | } 80 | 81 | ProbMapDisplay::~ProbMapDisplay() 82 | { 83 | unsubscribe(); 84 | clear(); 85 | } 86 | 87 | void ProbMapDisplay::onInitialize() 88 | { 89 | static int count = 0; 90 | std::stringstream ss; 91 | ss << "MapObjectMaterial" << count++; 92 | material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), 93 | Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); 94 | material_->setReceiveShadows(false); 95 | material_->getTechnique(0)->setLightingEnabled(false); 96 | material_->setDepthBias( -16.0f, 0.0f ); 97 | material_->setCullingMode( Ogre::CULL_NONE ); 98 | material_->setDepthWriteEnabled(false); 99 | 100 | updateAlpha(); 101 | } 102 | 103 | void ProbMapDisplay::onEnable() 104 | { 105 | subscribe(); 106 | } 107 | 108 | void ProbMapDisplay::onDisable() 109 | { 110 | unsubscribe(); 111 | clear(); 112 | } 113 | 114 | void ProbMapDisplay::subscribe() 115 | { 116 | if ( !isEnabled() ) 117 | { 118 | return; 119 | } 120 | 121 | if( !topic_property_->getTopic().isEmpty() ) 122 | { 123 | try 124 | { 125 | map_sub_ = update_nh_.subscribe( topic_property_->getTopicStd(), 1, &ProbMapDisplay::incomingMap, this ); 126 | setStatus( StatusProperty::Ok, "Topic", "OK" ); 127 | } 128 | catch( ros::Exception& e ) 129 | { 130 | setStatus( StatusProperty::Error, "Topic", QString( "Error subscribing: " ) + e.what() ); 131 | } 132 | } 133 | } 134 | 135 | void ProbMapDisplay::unsubscribe() 136 | { 137 | map_sub_.shutdown(); 138 | } 139 | 140 | void ProbMapDisplay::updateAlpha() 141 | { 142 | float alpha = alpha_property_->getFloat(); 143 | 144 | Ogre::Pass* pass = material_->getTechnique( 0 )->getPass( 0 ); 145 | Ogre::TextureUnitState* tex_unit = NULL; 146 | if( pass->getNumTextureUnitStates() > 0 ) 147 | { 148 | tex_unit = pass->getTextureUnitState( 0 ); 149 | } 150 | else 151 | { 152 | tex_unit = pass->createTextureUnitState(); 153 | } 154 | 155 | tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha ); 156 | 157 | if( alpha < 0.9998 ) 158 | { 159 | material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); 160 | material_->setDepthWriteEnabled( false ); 161 | } 162 | else 163 | { 164 | material_->setSceneBlending( Ogre::SBT_REPLACE ); 165 | material_->setDepthWriteEnabled( !draw_under_property_->getValue().toBool() ); 166 | } 167 | } 168 | 169 | void ProbMapDisplay::updateDrawUnder() 170 | { 171 | bool draw_under = draw_under_property_->getValue().toBool(); 172 | 173 | if( alpha_property_->getFloat() >= 0.9998 ) 174 | { 175 | material_->setDepthWriteEnabled( !draw_under ); 176 | } 177 | 178 | if( manual_object_ ) 179 | { 180 | if( draw_under ) 181 | { 182 | manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_4 ); 183 | } 184 | else 185 | { 186 | manual_object_->setRenderQueueGroup( Ogre::RENDER_QUEUE_MAIN ); 187 | } 188 | } 189 | } 190 | 191 | void ProbMapDisplay::updateTopic() 192 | { 193 | unsubscribe(); 194 | subscribe(); 195 | clear(); 196 | } 197 | 198 | void ProbMapDisplay::clear() 199 | { 200 | setStatus( StatusProperty::Warn, "Message", "No map received" ); 201 | 202 | if( !loaded_ ) 203 | { 204 | return; 205 | } 206 | 207 | scene_manager_->destroyManualObject( manual_object_ ); 208 | manual_object_ = NULL; 209 | 210 | std::string tex_name = texture_->getName(); 211 | texture_.setNull(); 212 | Ogre::TextureManager::getSingleton().unload( tex_name ); 213 | 214 | loaded_ = false; 215 | } 216 | 217 | bool validateFloats(const nav_msgs::OccupancyGrid& msg) 218 | { 219 | bool valid = true; 220 | valid = valid && validateFloats( msg.info.resolution ); 221 | valid = valid && validateFloats( msg.info.origin ); 222 | return valid; 223 | } 224 | 225 | void ProbMapDisplay::update( float wall_dt, float ros_dt ) 226 | { 227 | { 228 | boost::mutex::scoped_lock lock(mutex_); 229 | 230 | current_map_ = updated_map_; 231 | } 232 | 233 | if (!current_map_ || !new_map_) 234 | { 235 | return; 236 | } 237 | 238 | if (current_map_->data.empty()) 239 | { 240 | return; 241 | } 242 | 243 | new_map_ = false; 244 | 245 | if( !validateFloats( *current_map_ )) 246 | { 247 | setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" ); 248 | return; 249 | } 250 | 251 | if( current_map_->info.width * current_map_->info.height == 0 ) 252 | { 253 | std::stringstream ss; 254 | ss << "Map is zero-sized (" << current_map_->info.width << "x" << current_map_->info.height << ")"; 255 | setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() )); 256 | return; 257 | } 258 | 259 | clear(); 260 | 261 | setStatus( StatusProperty::Ok, "Message", "Map received" ); 262 | 263 | ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n", 264 | current_map_->info.width, 265 | current_map_->info.height, 266 | current_map_->info.resolution ); 267 | 268 | float resolution = current_map_->info.resolution; 269 | 270 | int width = current_map_->info.width; 271 | int height = current_map_->info.height; 272 | 273 | 274 | Ogre::Vector3 position( current_map_->info.origin.position.x, 275 | current_map_->info.origin.position.y, 276 | current_map_->info.origin.position.z ); 277 | Ogre::Quaternion orientation( current_map_->info.origin.orientation.w, 278 | current_map_->info.origin.orientation.x, 279 | current_map_->info.origin.orientation.y, 280 | current_map_->info.origin.orientation.z ); 281 | frame_ = current_map_->header.frame_id; 282 | if (frame_.empty()) 283 | { 284 | frame_ = "/map"; 285 | } 286 | 287 | // Expand it to be RGB data 288 | unsigned int pixels_size = width * height; 289 | unsigned char* pixels = new unsigned char[pixels_size]; 290 | memset(pixels, 255, pixels_size); 291 | 292 | bool map_status_set = false; 293 | unsigned int num_pixels_to_copy = pixels_size; 294 | if( pixels_size != current_map_->data.size() ) 295 | { 296 | std::stringstream ss; 297 | ss << "Data size doesn't match width*height: width = " << width 298 | << ", height = " << height << ", data size = " << current_map_->data.size(); 299 | setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() )); 300 | map_status_set = true; 301 | 302 | // Keep going, but don't read past the end of the data. 303 | if( current_map_->data.size() < pixels_size ) 304 | { 305 | num_pixels_to_copy = current_map_->data.size(); 306 | } 307 | } 308 | 309 | // TODO: a fragment shader could do this on the video card, and 310 | // would allow a non-grayscale color to mark the out-of-range 311 | // values. 312 | for( unsigned int pixel_index = 0; pixel_index < num_pixels_to_copy; pixel_index++ ) 313 | { 314 | unsigned char val; 315 | int8_t data = current_map_->data[ pixel_index ]; 316 | if(data > 0) 317 | val = 0; 318 | else if(data < 0) 319 | val = 255; 320 | else 321 | val = 127; 322 | pixels[ pixel_index ] = val; 323 | } 324 | 325 | Ogre::DataStreamPtr pixel_stream; 326 | pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size )); 327 | static int tex_count = 0; 328 | std::stringstream ss; 329 | ss << "MapTexture" << tex_count++; 330 | try 331 | { 332 | texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 333 | pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D, 334 | 0); 335 | 336 | if( !map_status_set ) 337 | { 338 | setStatus( StatusProperty::Ok, "Map", "Map OK" ); 339 | } 340 | } 341 | catch(Ogre::RenderingAPIException&) 342 | { 343 | Ogre::Image image; 344 | pixel_stream->seek(0); 345 | float fwidth = width; 346 | float fheight = height; 347 | if( width > height ) 348 | { 349 | float aspect = fheight / fwidth; 350 | fwidth = 2048; 351 | fheight = fwidth * aspect; 352 | } 353 | else 354 | { 355 | float aspect = fwidth / fheight; 356 | fheight = 2048; 357 | fwidth = fheight * aspect; 358 | } 359 | 360 | { 361 | std::stringstream ss; 362 | ss << "Map is larger than your graphics card supports. Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]"; 363 | setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() )); 364 | } 365 | 366 | ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048. Downsampling to [%d x %d]...", (int)fwidth, (int)fheight); 367 | //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height); 368 | image.loadRawData(pixel_stream, width, height, Ogre::PF_L8); 369 | image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST); 370 | ss << "Downsampled"; 371 | texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image); 372 | } 373 | 374 | delete [] pixels; 375 | 376 | Ogre::Pass* pass = material_->getTechnique(0)->getPass(0); 377 | Ogre::TextureUnitState* tex_unit = NULL; 378 | if (pass->getNumTextureUnitStates() > 0) 379 | { 380 | tex_unit = pass->getTextureUnitState(0); 381 | } 382 | else 383 | { 384 | tex_unit = pass->createTextureUnitState(); 385 | } 386 | 387 | tex_unit->setTextureName(texture_->getName()); 388 | tex_unit->setTextureFiltering( Ogre::TFO_NONE ); 389 | 390 | static int map_count = 0; 391 | std::stringstream ss2; 392 | ss2 << "MapObject" << map_count++; 393 | manual_object_ = scene_manager_->createManualObject( ss2.str() ); 394 | scene_node_->attachObject( manual_object_ ); 395 | 396 | manual_object_->begin(material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST); 397 | { 398 | 399 | { 400 | manual_object_->position( 0.0f, 0.0f, 0.0f ); 401 | manual_object_->textureCoord(0.0f, 0.0f); 402 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 403 | 404 | manual_object_->position( resolution*width, resolution*height, 0.0f ); 405 | manual_object_->textureCoord(1.0f, 1.0f); 406 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 407 | 408 | manual_object_->position( 0.0f, resolution*height, 0.0f ); 409 | manual_object_->textureCoord(0.0f, 1.0f); 410 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 411 | } 412 | 413 | { 414 | manual_object_->position( 0.0f, 0.0f, 0.0f ); 415 | manual_object_->textureCoord(0.0f, 0.0f); 416 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 417 | 418 | manual_object_->position( resolution*width, 0.0f, 0.0f ); 419 | manual_object_->textureCoord(1.0f, 0.0f); 420 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 421 | 422 | manual_object_->position( resolution*width, resolution*height, 0.0f ); 423 | manual_object_->textureCoord(1.0f, 1.0f); 424 | manual_object_->normal( 0.0f, 0.0f, 1.0f ); 425 | } 426 | } 427 | manual_object_->end(); 428 | 429 | if( draw_under_property_->getValue().toBool() ) 430 | { 431 | manual_object_->setRenderQueueGroup(Ogre::RENDER_QUEUE_4); 432 | } 433 | 434 | resolution_property_->setValue( resolution ); 435 | width_property_->setValue( width ); 436 | height_property_->setValue( height ); 437 | position_property_->setVector( position ); 438 | orientation_property_->setQuaternion( orientation ); 439 | 440 | transformMap(); 441 | 442 | loaded_ = true; 443 | 444 | context_->queueRender(); 445 | } 446 | 447 | void ProbMapDisplay::incomingMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) 448 | { 449 | 450 | updated_map_ = msg; 451 | boost::mutex::scoped_lock lock(mutex_); 452 | new_map_ = true; 453 | } 454 | 455 | 456 | 457 | void ProbMapDisplay::transformMap() 458 | { 459 | if (!current_map_) 460 | { 461 | return; 462 | } 463 | 464 | Ogre::Vector3 position; 465 | Ogre::Quaternion orientation; 466 | if (!context_->getFrameManager()->transform(frame_, ros::Time(), current_map_->info.origin, position, orientation)) 467 | { 468 | ROS_DEBUG( "Error transforming map '%s' from frame '%s' to frame '%s'", 469 | qPrintable( getName() ), frame_.c_str(), qPrintable( fixed_frame_ )); 470 | 471 | setStatus( StatusProperty::Error, "Transform", 472 | "No transform from [" + QString::fromStdString( frame_ ) + "] to [" + fixed_frame_ + "]" ); 473 | } 474 | else 475 | { 476 | setStatus(StatusProperty::Ok, "Transform", "Transform OK"); 477 | } 478 | 479 | scene_node_->setPosition( position ); 480 | scene_node_->setOrientation( orientation ); 481 | } 482 | 483 | void ProbMapDisplay::fixedFrameChanged() 484 | { 485 | transformMap(); 486 | } 487 | 488 | void ProbMapDisplay::reset() 489 | { 490 | Display::reset(); 491 | 492 | clear(); 493 | 494 | } 495 | 496 | } 497 | 498 | #include 499 | PLUGINLIB_EXPORT_CLASS( rviz::ProbMapDisplay, rviz::Display ) 500 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | --------------------------------------------------------------------------------