├── IRM.png ├── RMap.png ├── settings.png ├── rviz_reach_map.png ├── icons └── classes │ └── ReachabilityMap.png ├── msg ├── WorkSpace.msg └── WsSphere.msg ├── maps ├── 3D_reachability_map_arm_left_tool_link_0.05_2022.h5 ├── 3D_base_placement_map_arm_left_tool_link_0.05_2022.h5 ├── filtered_3D_reach_map_gripper_left_grasping_frame_torso_False_0.05_2022-08-29-19-04-04.npy └── filtered_3D_reach_map_gripper_right_grasping_frame_torso_False_0.05_2022-08-29-19-04-04.npy ├── plugin_description.xml ├── src ├── reachability_map_visual.h ├── reachability_map_display.h ├── load_reachability_map.cpp ├── reachability_map_display.cpp ├── reachability_map_visual.cpp └── hdf5_dataset.cpp ├── README.md ├── include └── reachability_map_visualizer │ └── hdf5_dataset.h ├── package.xml ├── CMakeLists.txt └── rviz ├── base_placement_map.rviz └── reachability_map.rviz /IRM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/IRM.png -------------------------------------------------------------------------------- /RMap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/RMap.png -------------------------------------------------------------------------------- /settings.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/settings.png -------------------------------------------------------------------------------- /rviz_reach_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/rviz_reach_map.png -------------------------------------------------------------------------------- /icons/classes/ReachabilityMap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/icons/classes/ReachabilityMap.png -------------------------------------------------------------------------------- /msg/WorkSpace.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | reachability_map_visualizer/WsSphere[] WsSpheres 4 | 5 | float32 resolution 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /msg/WsSphere.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | geometry_msgs/Point32 point 4 | 5 | float32 ri 6 | 7 | geometry_msgs/Pose[] poses 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /maps/3D_reachability_map_arm_left_tool_link_0.05_2022.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/maps/3D_reachability_map_arm_left_tool_link_0.05_2022.h5 -------------------------------------------------------------------------------- /maps/3D_base_placement_map_arm_left_tool_link_0.05_2022.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/maps/3D_base_placement_map_arm_left_tool_link_0.05_2022.h5 -------------------------------------------------------------------------------- /maps/filtered_3D_reach_map_gripper_left_grasping_frame_torso_False_0.05_2022-08-29-19-04-04.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/maps/filtered_3D_reach_map_gripper_left_grasping_frame_torso_False_0.05_2022-08-29-19-04-04.npy -------------------------------------------------------------------------------- /maps/filtered_3D_reach_map_gripper_right_grasping_frame_torso_False_0.05_2022-08-29-19-04-04.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pearl-robot-lab/reachability_map_visualizer/HEAD/maps/filtered_3D_reach_map_gripper_right_grasping_frame_torso_False_0.05_2022-08-29-19-04-04.npy -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Displays poses and spheres of reachability map 7 | 8 | reachability_map_visualizer::WorkSpace 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/reachability_map_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef ReachMap_VISUAL_H 2 | #define ReachMap_VISUAL_H 3 | 4 | #include 5 | 6 | namespace Ogre 7 | { 8 | class Vector3; 9 | class Quaternion; 10 | } 11 | 12 | namespace rviz 13 | { 14 | class Arrow; 15 | class Shape; 16 | } 17 | 18 | namespace reachability_map_visualizer 19 | { 20 | class ReachMapDisplay; 21 | class ReachMapVisual 22 | { 23 | public: 24 | ReachMapVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, rviz::DisplayContext* display); 25 | virtual ~ReachMapVisual(); 26 | void setMessage(const reachability_map_visualizer::WorkSpace::ConstPtr& msg, bool do_display_arrow, bool do_display_sphere, 27 | int low_ri, int high_ri, int shape_choice, int disect_choice); 28 | void setFramePosition(const Ogre::Vector3& position); 29 | void setFrameOrientation(const Ogre::Quaternion& orientation); 30 | 31 | void setColorArrow(float r, float g, float b, float a); 32 | void setSizeArrow(float l); 33 | 34 | void setColorSphere(float r, float g, float b, float a); 35 | void setSizeSphere(float l); 36 | void setColorSpherebyRI(float alpha); 37 | 38 | private: 39 | std::vector< boost::shared_ptr< rviz::Arrow > > arrow_; 40 | std::vector< boost::shared_ptr< rviz::Shape > > sphere_; 41 | std::vector< int > colorRI_; 42 | 43 | Ogre::SceneNode* frame_node_; 44 | 45 | Ogre::SceneManager* scene_manager_; 46 | }; 47 | } // end namespace reachability_map_visualizer 48 | #endif // ReachMap_VISUAL_H 49 | -------------------------------------------------------------------------------- /src/reachability_map_display.h: -------------------------------------------------------------------------------- 1 | #ifndef REACHABILITY_MAP_DISPLAY_H 2 | #define REACHABILITY_MAP_DISPLAY_H 3 | 4 | #ifndef Q_MOC_RUN 5 | 6 | #include 7 | 8 | #include 9 | #endif 10 | 11 | namespace Ogre 12 | { 13 | class SceneNode; 14 | } 15 | 16 | namespace rviz 17 | { 18 | class EnumProperty; 19 | class BoolProperty; 20 | class ColorProperty; 21 | class FloatProperty; 22 | class IntProperty; 23 | } 24 | 25 | namespace reachability_map_visualizer 26 | { 27 | class ReachMapVisual; 28 | class ReachMapDisplay : public rviz::MessageFilterDisplay< reachability_map_visualizer::WorkSpace > 29 | { 30 | Q_OBJECT 31 | public: 32 | enum Shape 33 | { 34 | Sphere, 35 | Cylinder, 36 | Cone, 37 | Cube, 38 | }; 39 | 40 | enum Disect 41 | { 42 | Full, 43 | First_Half, 44 | Second_Half, 45 | Middle_Slice, 46 | End_Slice, 47 | }; 48 | 49 | ReachMapDisplay(); 50 | virtual ~ReachMapDisplay(); 51 | 52 | protected: 53 | virtual void onInitialize(); 54 | virtual void reset(); 55 | 56 | private Q_SLOTS: 57 | void updateColorAndAlphaArrow(); 58 | void updateArrowSize(); 59 | 60 | void updateColorAndAlphaSphere(); 61 | void updateSphereSize(); 62 | 63 | private: 64 | void processMessage(const reachability_map_visualizer::WorkSpace::ConstPtr& msg); 65 | std::vector< boost::shared_ptr< ReachMapVisual > > visuals_; 66 | 67 | rviz::Property* arrow_category_; 68 | rviz::Property* sphere_category_; 69 | 70 | rviz::BoolProperty* do_display_arrow_; 71 | rviz::ColorProperty* arrow_color_property_; 72 | rviz::FloatProperty* arrow_alpha_property_; 73 | rviz::FloatProperty* arrow_length_property_; 74 | 75 | rviz::BoolProperty* do_display_sphere_; 76 | rviz::ColorProperty* sphere_color_property_; 77 | rviz::FloatProperty* sphere_alpha_property_; 78 | rviz::FloatProperty* sphere_radius_property_; 79 | 80 | rviz::IntProperty* lower_bound_reachability_; 81 | rviz::IntProperty* upper_bound_reachability_; 82 | rviz::BoolProperty* is_byReachability_; 83 | rviz::EnumProperty* shape_property_; 84 | rviz::EnumProperty* disect_property_; 85 | }; 86 | 87 | } // end namespace reachability_map_visualizer 88 | #endif // REACHABILITY_MAP_DISPLAY_H 89 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Reachability map visualizer 2 | 3 |

4 | 5 | 6 |

7 | 8 | **ROS** package for visualizing a robot's reachability and base placement maps 9 | 10 | - Currently supports ROS melodic and Ubuntu 18 (although porting to ROS noetic and Ubuntu 20 should not be difficult). 11 | 12 | - To generate the reachability maps, have a look at the [sampled_reachability_maps](https://github.com/iROSA-lab/sampled_reachability_maps) ([github.com/iROSA-lab/sampled_reachability_maps](https://github.com/iROSA-lab/sampled_reachability_maps)) repository. 13 | 14 | - This repository is a part of the codebase of the paper: **Robot Learning of Mobile Manipulation With Reachability Behavior Priors** [1] [[Paper](https://arxiv.org/abs/2203.04051)] [[Project site](https://irosalab.com/rlmmbp/)] 15 | 16 | - The code is based on the *workspace_visualization* package of the *reuleaux* repository ([github.com/ros-industrial-consortium/reuleaux](https://github.com/ros-industrial-consortium/reuleaux)). Please refer to [wiki.ros.org/reuleaux](http://wiki.ros.org/reuleaux) for more information. 17 | 18 | ## Loading a map and running the visualization 19 | 20 | - Build this package in your ROS workspace and source the `devel/setup.bash` file 21 | - Load the reachability/base_placement map by running (for example) 22 | ``` 23 | rosrun reachability_map_visualizer load_reachability_map $(rospack find reachability_map_visualizer)/maps/3D_reachability_map_arm_left_tool_link_0.05_2022.h5 24 | ``` 25 | - Run `rviz` and in the 'Displays' panel click on 'Add'. Choose 'By display type' and select the **ReachabilityMap** option under the 'reachability_map_visualizer' folder. To run our example rviz config, use: 26 | ``` 27 | rosrun rviz rviz -d $(rospack find reachability_map_visualizer)/rviz/reachability_map.rviz 28 | ``` 29 | 30 | - You can play with the visualization settings in Rviz: 31 | 32 | 33 | - The voxel coloring scheme can be changed by editing the "ReachMapVisual::setColorSpherebyRI" function in the "reachability_map_visual "file. By default, dark blue denotes the highest reachability while red denotes the lowest. 34 | 35 | ## References 36 | [1] S. Jauhri, J. Peters and G. Chalvatzaki, "Robot Learning of Mobile Manipulation With Reachability Behavior Priors", https://doi.org/10.1109/LRA.2022.3188109 -------------------------------------------------------------------------------- /src/load_reachability_map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "reachability_map_visualizer/WorkSpace.h" 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | if (argc < 2) 8 | { 9 | ROS_ERROR_STREAM("Please provide the name of the reachability map. If you have not created it yet, Please create " 10 | "the map by running the create reachability map launch file in reachability_map_visualizer"); 11 | return 1; 12 | } 13 | else 14 | { 15 | ros::init(argc, argv, "workspace"); 16 | ros::NodeHandle n; 17 | 18 | // TODO: It can be published as a latched topic. So the whole message will be published just once and stay on the 19 | // topic 20 | ros::Publisher workspace_pub = n.advertise< reachability_map_visualizer::WorkSpace >("reachability_map", 1); 21 | // bool latchOn = 1; 22 | // ros::Publisher workspace_pub = n.advertise("reachability_map", 1, latchOn); 23 | ros::Rate loop_rate(10); 24 | 25 | int count = 0; 26 | 27 | hdf5_dataset::Hdf5Dataset h5(argv[1]); 28 | h5.open(); 29 | 30 | MultiMapPtr pose_col_filter; 31 | MapVecDoublePtr sphere_col; 32 | float res; 33 | h5.loadMapsFromDataset(pose_col_filter, sphere_col, res); 34 | 35 | ROS_INFO("Map loading complete"); 36 | // Creating messages 37 | reachability_map_visualizer::WorkSpace ws; 38 | ws.header.stamp = ros::Time::now(); 39 | ws.header.frame_id = "/base_footprint"; 40 | ws.resolution = res; 41 | 42 | for (MapVecDoublePtr::iterator it = sphere_col.begin(); it != sphere_col.end(); ++it) 43 | { 44 | reachability_map_visualizer::WsSphere wss; 45 | wss.point.x = (*it->first)[0]; 46 | wss.point.y = (*it->first)[1]; 47 | wss.point.z = (*it->first)[2]; 48 | wss.ri = it->second; 49 | 50 | for (MultiMapPtr::iterator it1 = pose_col_filter.lower_bound(it->first); it1 != pose_col_filter.upper_bound(it->first); ++it1) 51 | { 52 | geometry_msgs::Pose pp; 53 | pp.position.x = it1->second->at(0); 54 | pp.position.y = it1->second->at(1); 55 | pp.position.z = it1->second->at(2); 56 | pp.orientation.x = it1->second->at(3); 57 | pp.orientation.y = it1->second->at(4); 58 | pp.orientation.z = it1->second->at(5); 59 | pp.orientation.w = it1->second->at(6); 60 | wss.poses.push_back(pp); 61 | } 62 | ws.WsSpheres.push_back(wss); 63 | } 64 | 65 | while (ros::ok()) 66 | { 67 | workspace_pub.publish(ws); 68 | 69 | ros::spinOnce(); 70 | sleep(5); 71 | ++count; 72 | } 73 | } 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /include/reachability_map_visualizer/hdf5_dataset.h: -------------------------------------------------------------------------------- 1 | #ifndef HDF5_DATASET_H 2 | #define HDF5_DATASET_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | typedef std::multimap< const std::vector< double >*, const std::vector< double >* > MultiMapPtr; 12 | typedef std::map< const std::vector< double >*, double > MapVecDoublePtr; 13 | typedef std::multimap< std::vector< double >, std::vector< double > > MultiMap; 14 | typedef std::map< std::vector< double >, double > MapVecDouble; 15 | typedef std::vector > VectorOfVectors; 16 | struct stat st; 17 | 18 | 19 | 20 | namespace hdf5_dataset 21 | { 22 | class Hdf5Dataset 23 | { 24 | public: 25 | Hdf5Dataset(std::string path, std::string filename); //Constructor for accessing .h5 file with path and filename 26 | Hdf5Dataset(std::string fullpath);// Constructor for accessing .h5 file with only fullpath 27 | 28 | //~Hdf5Dataset(); 29 | bool open(); //Opening the file, groups and database 30 | void close(); //Closing all the resources 31 | 32 | bool saveReachMapsToDataset( MultiMapPtr& poses, MapVecDoublePtr& spheres, float resolution); //Saves Mutimap and Map to database and closes 33 | 34 | bool loadMapsFromDataset(MultiMapPtr& poses, MapVecDoublePtr& spheres); //Creates exact same Poses MultiMap that was stored with address variation 35 | bool loadMapsFromDataset(MultiMapPtr& poses, MapVecDoublePtr& spheres, float &resolution); //with resolution 36 | bool loadMapsFromDataset(MultiMap& poses, MapVecDouble& spheres); //Loads the pose and sphere 37 | bool loadMapsFromDataset(MultiMap& Poses, MapVecDouble& Spheres, float &resolution); //with resolution 38 | bool h5ToResolution(float &resolution);//Accesses the resolution of the map 39 | 40 | private: 41 | 42 | bool h5ToMultiMapPosesAndSpheres(MultiMapPtr& pose_col, MapVecDoublePtr& sphere_col); //loads the whole data with same address structure as stored in .h5 43 | 44 | bool h5ToMultiMapPoses(MultiMap& pose_col, MapVecDouble& sphere_col); //accesses the poses and spheres data in the poses dataset 45 | bool h5ToMultiMapPoses(MultiMap& pose_col); //Accessess only the data from poses dataset regardless of address 46 | bool h5ToMultiMapSpheres(MapVecDouble& sphere_col); //Accessess only the data from spheres dataset regardless of address 47 | 48 | bool checkPath(std::string path); //Checking if path exists 49 | bool checkFileName(std::string filename); //Checking if filename is a .h5 or not 50 | void createPath(std::string path); //Creating the path 51 | 52 | 53 | 54 | std::string path_; 55 | std::string filename_; 56 | hid_t file_, group_poses_, group_spheres_; 57 | hid_t poses_dataset_, sphere_dataset_; 58 | hid_t attr_; 59 | float res_; 60 | 61 | }; 62 | 63 | } // namespace hdf5_dataset 64 | #endif // HDF5_DATASET_H 65 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | reachability_map_visualizer 4 | 0.0.0 5 | The reachability_map_visualizer package 6 | 7 | 8 | 9 | 10 | snehal jauhri 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | https://github.com/iROSA-lab/reachability_map_visualizer 22 | https://github.com/iROSA-lab/reachability_map_visualizer/issues 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 | catkin 54 | 55 | rviz 56 | qtbase5-dev 57 | 58 | rviz 59 | libqt5-core 60 | libqt5-gui 61 | libqt5-widgets 62 | hdf5 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/reachability_map_display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "reachability_map_visual.h" 14 | 15 | #include "reachability_map_display.h" 16 | 17 | namespace reachability_map_visualizer 18 | { 19 | ReachMapDisplay::ReachMapDisplay() 20 | { 21 | do_display_arrow_ = new rviz::BoolProperty("Show Poses", false, "Displays the arrow.", this); 22 | do_display_sphere_ = new rviz::BoolProperty("Show Shape", true, "Displays the spheres.", this); 23 | is_byReachability_ = new rviz::BoolProperty("Color by Reachability", true, "Color transform by Reachability Index", this); 24 | 25 | shape_property_ = new rviz::EnumProperty("Shape", "Sphere", "Shape to display the workspace.", this, 26 | SLOT(updateColorAndAlphaArrow())); 27 | shape_property_->addOption("Sphere", Sphere); 28 | shape_property_->addOption("Cylinder", Cylinder); 29 | shape_property_->addOption("Cone", Cone); 30 | shape_property_->addOption("Cube", Cube); 31 | 32 | disect_property_ = 33 | new rviz::EnumProperty("Disect", "Full", "Disection of the workspace", this, SLOT(updateColorAndAlphaArrow())); 34 | disect_property_->addOption("Full", Full); 35 | disect_property_->addOption("1st_Half", First_Half); 36 | disect_property_->addOption("2nd_Half", Second_Half); 37 | disect_property_->addOption("Middle_Slice", Middle_Slice); 38 | disect_property_->addOption("End_Slice", End_Slice); 39 | 40 | // Arrow Property category 41 | arrow_category_ = new rviz::Property("Poses Property", QVariant(), "", this); 42 | 43 | arrow_color_property_ = new rviz::ColorProperty("Color", QColor(204, 51, 204), "Color to draw the Pose arrows.", 44 | arrow_category_, SLOT(updateColorAndAlphaArrow()), this); 45 | 46 | arrow_alpha_property_ = new rviz::FloatProperty("Alpha", 0.2, "0 is fully transparent, 1.0 is fully opaque.", 47 | arrow_category_, SLOT(updateColorAndAlphaArrow()), this); 48 | 49 | arrow_length_property_ = 50 | new rviz::FloatProperty("Length", 0.01, "Length of the arrows", arrow_category_, SLOT(updateArrowSize()), this); 51 | 52 | // Shape Property category 53 | 54 | sphere_category_ = new rviz::Property("Shape Property", QVariant(), "", this); 55 | 56 | sphere_color_property_ = new rviz::ColorProperty("Color", QColor(255, 225, 102), "Color to draw the Sphere.", 57 | sphere_category_, SLOT(updateColorAndAlphaSphere()), this); 58 | 59 | sphere_alpha_property_ = new rviz::FloatProperty("Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", 60 | sphere_category_, SLOT(updateColorAndAlphaSphere()), this); 61 | 62 | sphere_radius_property_ = 63 | new rviz::FloatProperty("Size", 0.05, "Size of the sphere", sphere_category_, SLOT(updateSphereSize()), this); 64 | 65 | lower_bound_reachability_ = new rviz::IntProperty("Lowest Reachability Index", 0, "Lowest Reachability index.", this); 66 | 67 | upper_bound_reachability_ = 68 | new rviz::IntProperty("Highest Reachability Index", 100, "Highest Reachability index.", this); 69 | } 70 | 71 | void ReachMapDisplay::onInitialize() 72 | { 73 | MFDClass::onInitialize(); 74 | } 75 | 76 | ReachMapDisplay::~ReachMapDisplay() 77 | { 78 | } 79 | void ReachMapDisplay::reset() 80 | { 81 | MFDClass::reset(); 82 | visuals_.clear(); 83 | } 84 | 85 | void ReachMapDisplay::updateColorAndAlphaArrow() 86 | { 87 | float alpha = arrow_alpha_property_->getFloat(); 88 | Ogre::ColourValue color = arrow_color_property_->getOgreColor(); 89 | 90 | for (size_t i = 0; i < visuals_.size(); i++) 91 | { 92 | visuals_[i]->setColorArrow(color.r, color.g, color.b, alpha); 93 | } 94 | } 95 | 96 | void ReachMapDisplay::updateArrowSize() 97 | { 98 | float length = arrow_length_property_->getFloat(); 99 | for (size_t i = 0; i < visuals_.size(); i++) 100 | { 101 | visuals_[i]->setSizeArrow(length); 102 | } 103 | } 104 | 105 | void ReachMapDisplay::updateColorAndAlphaSphere() 106 | { 107 | float alpha = sphere_alpha_property_->getFloat(); 108 | Ogre::ColourValue color = sphere_color_property_->getOgreColor(); 109 | 110 | for (size_t i = 0; i < visuals_.size(); i++) 111 | { 112 | visuals_[i]->setColorSphere(color.r, color.g, color.b, alpha); 113 | } 114 | } 115 | 116 | void ReachMapDisplay::updateSphereSize() 117 | { 118 | float length = sphere_radius_property_->getFloat(); 119 | for (size_t i = 0; i < visuals_.size(); i++) 120 | { 121 | visuals_[i]->setSizeSphere(length); 122 | } 123 | } 124 | 125 | void ReachMapDisplay::processMessage(const reachability_map_visualizer::WorkSpace::ConstPtr& msg) 126 | { 127 | Ogre::Quaternion orientation; 128 | Ogre::Vector3 position; 129 | if (!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation)) 130 | { 131 | ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), 132 | qPrintable(fixed_frame_)); 133 | return; 134 | } 135 | boost::shared_ptr< ReachMapVisual > visual; 136 | visuals_.clear(); 137 | visual.reset(new ReachMapVisual(context_->getSceneManager(), scene_node_, context_)); 138 | 139 | visual->setMessage(msg, do_display_arrow_->getBool(), do_display_sphere_->getBool(), 140 | lower_bound_reachability_->getInt(), upper_bound_reachability_->getInt(), 141 | shape_property_->getOptionInt(), disect_property_->getOptionInt()); 142 | 143 | visual->setFramePosition(position); 144 | visual->setFrameOrientation(orientation); 145 | 146 | float arrow_alpha = arrow_alpha_property_->getFloat(); 147 | Ogre::ColourValue arrow_color = arrow_color_property_->getOgreColor(); 148 | visual->setColorArrow(arrow_color.r, arrow_color.g, arrow_color.b, arrow_alpha); 149 | 150 | float sphere_alpha = sphere_alpha_property_->getFloat(); 151 | Ogre::ColourValue sphere_color = sphere_color_property_->getOgreColor(); 152 | if (is_byReachability_->getBool() == false) 153 | { 154 | visual->setColorSphere(sphere_color.r, sphere_color.g, sphere_color.b, sphere_alpha); 155 | } 156 | else 157 | { 158 | visual->setColorSpherebyRI(sphere_alpha); 159 | } 160 | 161 | visuals_.push_back(visual); 162 | //updateArrowSize(); 163 | updateSphereSize(); 164 | 165 | } 166 | 167 | } // end namespace reachability_map_visualizer 168 | #include 169 | PLUGINLIB_EXPORT_CLASS(reachability_map_visualizer::ReachMapDisplay, rviz::Display) 170 | -------------------------------------------------------------------------------- /src/reachability_map_visual.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "reachability_map_visual.h" 13 | #include 14 | 15 | namespace reachability_map_visualizer 16 | { 17 | ReachMapVisual::ReachMapVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, 18 | rviz::DisplayContext* display_context) 19 | { 20 | scene_manager_ = scene_manager; 21 | frame_node_ = parent_node->createChildSceneNode(); 22 | 23 | // arrow_.reset(new rviz::Arrow( scene_manager_, frame_node_ )); 24 | } 25 | 26 | ReachMapVisual::~ReachMapVisual() 27 | { 28 | scene_manager_->destroySceneNode(frame_node_); 29 | } 30 | 31 | void ReachMapVisual::setMessage(const reachability_map_visualizer::WorkSpace::ConstPtr& msg, bool do_display_arrow, 32 | bool do_display_sphere, int low_ri, int high_ri, int shape_choice, int disect_choice) 33 | { 34 | int low_SphereSize, up_SphereSize; 35 | // TODO: Not a very delicate process to dissect the workspace. Implement a way to provide a range and allow the user 36 | // to select an axis and an upper/lower bound to slice with 37 | switch (disect_choice) 38 | { 39 | case 0: 40 | { 41 | low_SphereSize = 0; 42 | up_SphereSize = msg->WsSpheres.size(); 43 | break; 44 | } 45 | case 1: 46 | { 47 | low_SphereSize = 0; 48 | up_SphereSize = msg->WsSpheres.size() / 2; 49 | break; 50 | } 51 | case 2: 52 | { 53 | low_SphereSize = msg->WsSpheres.size() / 2; 54 | up_SphereSize = msg->WsSpheres.size(); 55 | break; 56 | } 57 | case 3: 58 | { 59 | low_SphereSize = msg->WsSpheres.size() / 2.2; 60 | up_SphereSize = msg->WsSpheres.size() / 1.8; 61 | break; 62 | } 63 | case 4: 64 | { 65 | low_SphereSize = 0; 66 | up_SphereSize = msg->WsSpheres.size() / 1.1; 67 | break; 68 | } 69 | } 70 | 71 | if (do_display_arrow) 72 | { 73 | boost::shared_ptr< rviz::Arrow > pose_arrow; 74 | for (size_t i = low_SphereSize; i < up_SphereSize; ++i) 75 | { 76 | for (size_t j = 0; j < msg->WsSpheres[i].poses.size(); ++j) 77 | { 78 | if (low_ri < int(msg->WsSpheres[i].ri) && int(msg->WsSpheres[i].ri) <= high_ri) 79 | { 80 | pose_arrow.reset(new rviz::Arrow(scene_manager_, frame_node_)); 81 | 82 | Ogre::Vector3 position_(msg->WsSpheres[i].poses[j].position.x, msg->WsSpheres[i].poses[j].position.y, 83 | msg->WsSpheres[i].poses[j].position.z); 84 | tf2::Quaternion quat(msg->WsSpheres[i].poses[j].orientation.x, msg->WsSpheres[i].poses[j].orientation.y, 85 | msg->WsSpheres[i].poses[j].orientation.z, msg->WsSpheres[i].poses[j].orientation.w); 86 | 87 | tf2::Quaternion q2; 88 | q2.setRPY(0, -M_PI / 2, 0); // Arrows are pointed as -z direction. So rotating it is necessary 89 | 90 | quat *= (q2); 91 | quat.normalize(); 92 | Ogre::Quaternion orientation_(quat.w(), quat.x(), quat.y(), quat.z()); 93 | 94 | if (position_.isNaN() || orientation_.isNaN()) 95 | { 96 | ROS_WARN("received invalid pose"); 97 | return; 98 | } 99 | 100 | pose_arrow->setPosition(position_); 101 | pose_arrow->setOrientation(orientation_); 102 | 103 | arrow_.push_back(pose_arrow); 104 | } 105 | } 106 | } 107 | } 108 | if (do_display_sphere) 109 | { 110 | boost::shared_ptr< rviz::Shape > sphere_center; 111 | int colorRI; 112 | 113 | for (size_t i = low_SphereSize; i < up_SphereSize; ++i) 114 | { 115 | if (low_ri <= int(msg->WsSpheres[i].ri) && int(msg->WsSpheres[i].ri <= high_ri)) 116 | { 117 | switch (shape_choice) 118 | { 119 | case 0: 120 | { 121 | sphere_center.reset(new rviz::Shape(rviz::Shape::Sphere, scene_manager_, frame_node_)); 122 | break; 123 | } 124 | case 1: 125 | { 126 | sphere_center.reset(new rviz::Shape(rviz::Shape::Cylinder, scene_manager_, frame_node_)); 127 | break; 128 | } 129 | case 2: 130 | { 131 | sphere_center.reset(new rviz::Shape(rviz::Shape::Cone, scene_manager_, frame_node_)); 132 | break; 133 | } 134 | case 3: 135 | { 136 | sphere_center.reset(new rviz::Shape(rviz::Shape::Cube, scene_manager_, frame_node_)); 137 | break; 138 | } 139 | } 140 | 141 | Ogre::Vector3 position_sphere(msg->WsSpheres[i].point.x, msg->WsSpheres[i].point.y, msg->WsSpheres[i].point.z); 142 | Ogre::Quaternion orientation_sphere(0, 0, 0, 1); 143 | colorRI = msg->WsSpheres[i].ri; 144 | if (position_sphere.isNaN()) 145 | { 146 | ROS_WARN("received invalid sphere coordinate"); 147 | return; 148 | } 149 | sphere_center->setPosition(position_sphere); 150 | sphere_center->setOrientation(orientation_sphere); 151 | 152 | sphere_.push_back(sphere_center); 153 | colorRI_.push_back(colorRI); 154 | } 155 | } 156 | } 157 | } 158 | 159 | void ReachMapVisual::setFramePosition(const Ogre::Vector3& position) 160 | { 161 | frame_node_->setPosition(position); 162 | } 163 | 164 | void ReachMapVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 165 | { 166 | frame_node_->setOrientation(orientation); 167 | } 168 | 169 | void ReachMapVisual::setColorArrow(float r, float g, float b, float a) 170 | { 171 | for (int i = 0; i < arrow_.size(); ++i) 172 | { 173 | arrow_[i]->setColor(r, g, b, a); 174 | } 175 | } 176 | 177 | void ReachMapVisual::setColorSphere(float r, float g, float b, float a) 178 | { 179 | for (int i = 0; i < sphere_.size(); ++i) 180 | { 181 | sphere_[i]->setColor(r, g, b, a); 182 | } 183 | } 184 | 185 | void ReachMapVisual::setColorSpherebyRI(float alpha) 186 | { 187 | for (int i = 0; i < sphere_.size(); ++i) 188 | { 189 | if (colorRI_[i] >= 90) 190 | { 191 | sphere_[i]->setColor(0, 0, 255, alpha); 192 | } 193 | else if (colorRI_[i] < 90 && colorRI_[i] >= 50) 194 | { 195 | sphere_[i]->setColor(0, 255, 255, alpha); 196 | } 197 | else if (colorRI_[i] < 50 && colorRI_[i] >= 30) 198 | { 199 | sphere_[i]->setColor(0, 255, 0, alpha); 200 | } 201 | else if (colorRI_[i] < 30 && colorRI_[i] >= 5) 202 | { 203 | sphere_[i]->setColor(255, 255, 0, alpha); 204 | } 205 | else 206 | { 207 | sphere_[i]->setColor(255, 0, 0, alpha); 208 | } 209 | } 210 | } 211 | 212 | void ReachMapVisual::setSizeArrow(float l) 213 | { 214 | for (int i = 0; i < arrow_.size(); ++i) 215 | { 216 | arrow_[i]->setScale(Ogre::Vector3(l, l, l)); 217 | } 218 | } 219 | 220 | void ReachMapVisual::setSizeSphere(float l) 221 | { 222 | for (int i = 0; i < sphere_.size(); ++i) 223 | { 224 | sphere_[i]->setScale(Ogre::Vector3(l, l, l)); 225 | } 226 | } 227 | 228 | } // end namespace reachability_map_visualizer 229 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(reachability_map_visualizer) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rviz 12 | std_msgs 13 | geometry_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | find_package(HDF5 REQUIRED) 19 | find_package(MPI REQUIRED) 20 | 21 | ## Qt setup: 22 | set(CMAKE_AUTOMOC ON) 23 | 24 | if(rviz_QT_VERSION VERSION_LESS "5") 25 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 26 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 27 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 28 | include(${QT_USE_FILE}) 29 | else() 30 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 31 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 32 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 33 | set(QT_LIBRARIES Qt5::Widgets) 34 | endif() 35 | 36 | add_definitions(-DQT_NO_KEYWORDS) 37 | 38 | ## Uncomment this if the package has a setup.py. This macro ensures 39 | ## modules and global scripts declared therein get installed 40 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 41 | # catkin_python_setup() 42 | 43 | ################################################ 44 | ## Declare ROS messages, services and actions ## 45 | ################################################ 46 | 47 | ## To declare and build messages, services or actions from within this 48 | ## package, follow these steps: 49 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 50 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 51 | ## * In the file package.xml: 52 | ## * add a build_depend tag for "message_generation" 53 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 54 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 55 | ## but can be declared for certainty nonetheless: 56 | ## * add a exec_depend tag for "message_runtime" 57 | ## * In this file (CMakeLists.txt): 58 | ## * add "message_generation" and every package in MSG_DEP_SET to 59 | ## find_package(catkin REQUIRED COMPONENTS ...) 60 | ## * add "message_runtime" and every package in MSG_DEP_SET to 61 | ## catkin_package(CATKIN_DEPENDS ...) 62 | ## * uncomment the add_*_files sections below as needed 63 | ## and list every .msg/.srv/.action file to be processed 64 | ## * uncomment the generate_messages entry below 65 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 66 | 67 | ## Generate messages in the 'msg' folder 68 | add_message_files( 69 | FILES 70 | WsSphere.msg 71 | WorkSpace.msg 72 | ) 73 | 74 | ## Generate services in the 'srv' folder 75 | # add_service_files( 76 | # FILES 77 | # Service1.srv 78 | # Service2.srv 79 | # ) 80 | 81 | ## Generate actions in the 'action' folder 82 | # add_action_files( 83 | # FILES 84 | # Action1.action 85 | # Action2.action 86 | # ) 87 | 88 | ## Generate added messages and services with any dependencies listed here 89 | generate_messages( 90 | DEPENDENCIES 91 | geometry_msgs 92 | std_msgs # Or other packages containing msgs 93 | ) 94 | 95 | ################################################ 96 | ## Declare ROS dynamic reconfigure parameters ## 97 | ################################################ 98 | 99 | ## To declare and build dynamic reconfigure parameters within this 100 | ## package, follow these steps: 101 | ## * In the file package.xml: 102 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 103 | ## * In this file (CMakeLists.txt): 104 | ## * add "dynamic_reconfigure" to 105 | ## find_package(catkin REQUIRED COMPONENTS ...) 106 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 107 | ## and list every .cfg file to be processed 108 | 109 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 110 | # generate_dynamic_reconfigure_options( 111 | # cfg/DynReconf1.cfg 112 | # cfg/DynReconf2.cfg 113 | # ) 114 | 115 | ################################### 116 | ## catkin specific configuration ## 117 | ################################### 118 | ## The catkin_package macro generates cmake config files for your package 119 | ## Declare things to be passed to dependent projects 120 | ## INCLUDE_DIRS: uncomment this if your package contains header files 121 | ## LIBRARIES: libraries you create in this project that dependent projects also need 122 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 123 | ## DEPENDS: system dependencies of this project that dependent projects also need 124 | catkin_package( 125 | INCLUDE_DIRS 126 | include 127 | LIBRARIES 128 | hdf5_dataset 129 | roscpp 130 | CATKIN_DEPENDS message_runtime 131 | # DEPENDS system_lib 132 | ) 133 | 134 | ########### 135 | ## Build ## 136 | ########### 137 | 138 | ## Specify additional locations of header files 139 | ## Your package locations should be listed before other locations 140 | include_directories( 141 | include 142 | ${catkin_INCLUDE_DIRS} 143 | ${HDF5_INCLUDE_DIRS} 144 | ${MPI_CXX_INCLUDE_DIRS} 145 | ) 146 | 147 | ## Declare a C++ library 148 | add_library(${PROJECT_NAME} 149 | src/reachability_map_visual.cpp 150 | src/reachability_map_display.cpp 151 | ) 152 | add_library(hdf5_dataset 153 | src/hdf5_dataset.cpp 154 | ) 155 | 156 | ## Add cmake target dependencies of the library 157 | ## as an example, code may need to be generated before libraries 158 | ## either from message generation or dynamic reconfigure 159 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 160 | 161 | ## Declare a C++ executable 162 | ## With catkin_make all packages are built within a single CMake context 163 | ## The recommended prefix ensures that target names across packages don't collide 164 | add_executable(load_reachability_map src/load_reachability_map.cpp) 165 | add_dependencies(load_reachability_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 166 | 167 | ## Rename C++ executable without prefix 168 | ## The above recommended prefix causes long target names, the following renames the 169 | ## target back to the shorter version for ease of user use 170 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 171 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 172 | 173 | ## Add cmake target dependencies of the executable 174 | ## same as for the library above 175 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 176 | 177 | ## Specify libraries to link a library or executable target against 178 | target_link_libraries(${PROJECT_NAME} 179 | ${QT_LIBRARIES} 180 | ${catkin_LIBRARIES} 181 | ) 182 | 183 | target_link_libraries(hdf5_dataset 184 | ${catkin_LIBRARIES} 185 | ${HDF5_LIBRARIES} 186 | ${MPI_CXX_LIBRARIES} 187 | ) 188 | 189 | target_link_libraries(load_reachability_map hdf5_dataset ${catkin_LIBRARIES}) 190 | 191 | ############# 192 | ## Install ## 193 | ############# 194 | 195 | # all install targets should use catkin DESTINATION variables 196 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 197 | 198 | ## Mark executable scripts (Python etc.) for installation 199 | ## in contrast to setup.py, you can choose the destination 200 | # catkin_install_python(PROGRAMS 201 | # scripts/my_python_script 202 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 203 | # ) 204 | 205 | ## Mark executables for installation 206 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 207 | # install(TARGETS ${PROJECT_NAME}_node 208 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 209 | # ) 210 | 211 | ## Mark libraries for installation 212 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 213 | install(TARGETS ${PROJECT_NAME} hdf5_dataset 214 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 215 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 216 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 217 | ) 218 | 219 | ## Mark cpp header files for installation 220 | # install(DIRECTORY include/${PROJECT_NAME}/ 221 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 222 | # FILES_MATCHING PATTERN "*.h" 223 | # PATTERN ".svn" EXCLUDE 224 | # ) 225 | install(DIRECTORY include/${PROJECT_NAME}/ 226 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 227 | ) 228 | 229 | 230 | ## Mark other files for installation (e.g. launch and bag files, etc.) 231 | install(FILES 232 | plugin_description.xml 233 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 234 | 235 | install(DIRECTORY icons/ 236 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 237 | 238 | ############# 239 | ## Testing ## 240 | ############# 241 | 242 | ## Add gtest based cpp test target and link libraries 243 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_reachability_map_visualizer.cpp) 244 | # if(TARGET ${PROJECT_NAME}-test) 245 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 246 | # endif() 247 | 248 | ## Add folders to be run by python nosetests 249 | # catkin_add_nosetests(test) 250 | -------------------------------------------------------------------------------- /rviz/base_placement_map.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /ReachabilityMap1 10 | - /ReachabilityMap1/Shape Property1 11 | Splitter Ratio: 0.5 12 | Tree Height: 746 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 1 40 | Class: rviz/RobotModel 41 | Collision Enabled: false 42 | Enabled: true 43 | Links: 44 | All Links Enabled: true 45 | Expand Joint Details: false 46 | Expand Link Details: false 47 | Expand Tree: false 48 | Link Tree Style: Links in Alphabetic Order 49 | arm_left_1_link: 50 | Alpha: 1 51 | Show Axes: false 52 | Show Trail: false 53 | Value: true 54 | arm_left_2_link: 55 | Alpha: 1 56 | Show Axes: false 57 | Show Trail: false 58 | Value: true 59 | arm_left_3_link: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | Value: true 64 | arm_left_4_link: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | arm_left_5_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | arm_left_6_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | arm_left_7_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | arm_left_tool_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | arm_right_1_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | arm_right_2_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | arm_right_3_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | arm_right_4_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | arm_right_5_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | arm_right_6_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | arm_right_7_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | arm_right_tool_link: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | Value: true 127 | base_antenna_left_link: 128 | Alpha: 1 129 | Show Axes: false 130 | Show Trail: false 131 | Value: true 132 | base_antenna_right_link: 133 | Alpha: 1 134 | Show Axes: false 135 | Show Trail: false 136 | Value: true 137 | base_cover_link: 138 | Alpha: 1 139 | Show Axes: false 140 | Show Trail: false 141 | Value: true 142 | base_footprint: 143 | Alpha: 1 144 | Show Axes: false 145 | Show Trail: false 146 | base_imu_link: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | base_laser_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | base_link: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | base_mic_back_left_link: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | base_mic_back_right_link: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | base_mic_front_left_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | base_mic_front_right_link: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | base_sonar_01_link: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | base_sonar_02_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | base_sonar_03_link: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | Value: true 191 | caster_back_left_1_link: 192 | Alpha: 1 193 | Show Axes: false 194 | Show Trail: false 195 | Value: true 196 | caster_back_left_2_link: 197 | Alpha: 1 198 | Show Axes: false 199 | Show Trail: false 200 | Value: true 201 | caster_back_right_1_link: 202 | Alpha: 1 203 | Show Axes: false 204 | Show Trail: false 205 | Value: true 206 | caster_back_right_2_link: 207 | Alpha: 1 208 | Show Axes: false 209 | Show Trail: false 210 | Value: true 211 | caster_front_left_1_link: 212 | Alpha: 1 213 | Show Axes: false 214 | Show Trail: false 215 | Value: true 216 | caster_front_left_2_link: 217 | Alpha: 1 218 | Show Axes: false 219 | Show Trail: false 220 | Value: true 221 | caster_front_right_1_link: 222 | Alpha: 1 223 | Show Axes: false 224 | Show Trail: false 225 | Value: true 226 | caster_front_right_2_link: 227 | Alpha: 1 228 | Show Axes: false 229 | Show Trail: false 230 | Value: true 231 | gripper_left_grasping_frame: 232 | Alpha: 1 233 | Show Axes: false 234 | Show Trail: false 235 | gripper_left_left_finger_link: 236 | Alpha: 1 237 | Show Axes: false 238 | Show Trail: false 239 | Value: true 240 | gripper_left_link: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | Value: true 245 | gripper_left_right_finger_link: 246 | Alpha: 1 247 | Show Axes: false 248 | Show Trail: false 249 | Value: true 250 | gripper_left_tool_link: 251 | Alpha: 1 252 | Show Axes: false 253 | Show Trail: false 254 | gripper_right_grasping_frame: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | gripper_right_left_finger_link: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | Value: true 263 | gripper_right_link: 264 | Alpha: 1 265 | Show Axes: false 266 | Show Trail: false 267 | Value: true 268 | gripper_right_right_finger_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | Value: true 273 | gripper_right_tool_link: 274 | Alpha: 1 275 | Show Axes: false 276 | Show Trail: false 277 | head_1_link: 278 | Alpha: 1 279 | Show Axes: false 280 | Show Trail: false 281 | Value: true 282 | head_2_link: 283 | Alpha: 1 284 | Show Axes: false 285 | Show Trail: false 286 | Value: true 287 | rgbd_laser_link: 288 | Alpha: 1 289 | Show Axes: false 290 | Show Trail: false 291 | suspension_left_link: 292 | Alpha: 1 293 | Show Axes: false 294 | Show Trail: false 295 | suspension_right_link: 296 | Alpha: 1 297 | Show Axes: false 298 | Show Trail: false 299 | torso_fixed_column_link: 300 | Alpha: 1 301 | Show Axes: false 302 | Show Trail: false 303 | Value: true 304 | torso_fixed_link: 305 | Alpha: 1 306 | Show Axes: false 307 | Show Trail: false 308 | Value: true 309 | torso_lift_link: 310 | Alpha: 1 311 | Show Axes: false 312 | Show Trail: false 313 | Value: true 314 | wheel_left_link: 315 | Alpha: 1 316 | Show Axes: false 317 | Show Trail: false 318 | Value: true 319 | wheel_right_link: 320 | Alpha: 1 321 | Show Axes: false 322 | Show Trail: false 323 | Value: true 324 | wrist_left_ft_link: 325 | Alpha: 1 326 | Show Axes: false 327 | Show Trail: false 328 | Value: true 329 | wrist_left_ft_tool_link: 330 | Alpha: 1 331 | Show Axes: false 332 | Show Trail: false 333 | Value: true 334 | wrist_right_ft_link: 335 | Alpha: 1 336 | Show Axes: false 337 | Show Trail: false 338 | Value: true 339 | wrist_right_ft_tool_link: 340 | Alpha: 1 341 | Show Axes: false 342 | Show Trail: false 343 | Value: true 344 | xtion_depth_frame: 345 | Alpha: 1 346 | Show Axes: false 347 | Show Trail: false 348 | xtion_depth_optical_frame: 349 | Alpha: 1 350 | Show Axes: false 351 | Show Trail: false 352 | xtion_link: 353 | Alpha: 1 354 | Show Axes: false 355 | Show Trail: false 356 | xtion_optical_frame: 357 | Alpha: 1 358 | Show Axes: false 359 | Show Trail: false 360 | xtion_orbbec_aux_joint_frame: 361 | Alpha: 1 362 | Show Axes: false 363 | Show Trail: false 364 | xtion_rgb_frame: 365 | Alpha: 1 366 | Show Axes: false 367 | Show Trail: false 368 | xtion_rgb_optical_frame: 369 | Alpha: 1 370 | Show Axes: false 371 | Show Trail: false 372 | Name: RobotModel 373 | Robot Description: robot_description 374 | TF Prefix: "" 375 | Update Interval: 0 376 | Value: true 377 | Visual Enabled: true 378 | - Alpha: 0.5 379 | Cell Size: 1 380 | Class: rviz/Grid 381 | Color: 160; 160; 164 382 | Enabled: false 383 | Line Style: 384 | Line Width: 0.029999999329447746 385 | Value: Lines 386 | Name: Grid 387 | Normal Cell Count: 0 388 | Offset: 389 | X: 0 390 | Y: 0 391 | Z: 0 392 | Plane: XY 393 | Plane Cell Count: 10 394 | Reference Frame: 395 | Value: false 396 | - Class: reachability_map_visualizer/ReachabilityMap 397 | Color by Reachability: true 398 | Disect: Full 399 | Enabled: true 400 | Highest Reachability Index: 100 401 | Lowest Reachability Index: 0 402 | Name: ReachabilityMap 403 | Poses Property: 404 | Alpha: 0.20000000298023224 405 | Color: 204; 51; 204 406 | Length: 0.009999999776482582 407 | Shape: Sphere 408 | Shape Property: 409 | Alpha: 0.05000000074505806 410 | Color: 255; 225; 102 411 | Size: 0.05000000074505806 412 | Show Poses: false 413 | Show Shape: true 414 | Topic: /reachability_map 415 | Unreliable: false 416 | Value: true 417 | Enabled: true 418 | Global Options: 419 | Background Color: 255; 255; 255 420 | Default Light: true 421 | Fixed Frame: base_footprint 422 | Frame Rate: 30 423 | Name: root 424 | Tools: 425 | - Class: rviz/Interact 426 | Hide Inactive Objects: true 427 | - Class: rviz/MoveCamera 428 | - Class: rviz/Select 429 | - Class: rviz/FocusCamera 430 | - Class: rviz/Measure 431 | - Class: rviz/SetInitialPose 432 | Theta std deviation: 0.2617993950843811 433 | Topic: /initialpose 434 | X std deviation: 0.5 435 | Y std deviation: 0.5 436 | - Class: rviz/SetGoal 437 | Topic: /move_base_simple/goal 438 | - Class: rviz/PublishPoint 439 | Single click: true 440 | Topic: /clicked_point 441 | Value: true 442 | Views: 443 | Current: 444 | Class: rviz/Orbit 445 | Distance: 3.995306968688965 446 | Enable Stereo Rendering: 447 | Stereo Eye Separation: 0.05999999865889549 448 | Stereo Focal Distance: 1 449 | Swap Stereo Eyes: false 450 | Value: false 451 | Focal Point: 452 | X: -0.16442163288593292 453 | Y: 0.28310462832450867 454 | Z: 0.726998507976532 455 | Focal Shape Fixed Size: true 456 | Focal Shape Size: 0.05000000074505806 457 | Invert Z Axis: false 458 | Name: Current View 459 | Near Clip Distance: 0.009999999776482582 460 | Pitch: 0.2653980851173401 461 | Target Frame: 462 | Value: Orbit (rviz) 463 | Yaw: 0.2622203826904297 464 | Saved: ~ 465 | Window Geometry: 466 | Displays: 467 | collapsed: false 468 | Height: 1043 469 | Hide Left Dock: false 470 | Hide Right Dock: true 471 | QMainWindow State: 000000ff00000000fd0000000400000000000001bc00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000007800000023100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005be0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 472 | Selection: 473 | collapsed: false 474 | Time: 475 | collapsed: false 476 | Tool Properties: 477 | collapsed: false 478 | Views: 479 | collapsed: true 480 | Width: 1920 481 | X: 0 482 | Y: 561 483 | -------------------------------------------------------------------------------- /rviz/reachability_map.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /ReachabilityMap1 10 | - /ReachabilityMap1/Shape Property1 11 | Splitter Ratio: 0.5 12 | Tree Height: 746 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 1 40 | Class: rviz/RobotModel 41 | Collision Enabled: false 42 | Enabled: true 43 | Links: 44 | All Links Enabled: true 45 | Expand Joint Details: false 46 | Expand Link Details: false 47 | Expand Tree: false 48 | Link Tree Style: Links in Alphabetic Order 49 | arm_left_1_link: 50 | Alpha: 1 51 | Show Axes: false 52 | Show Trail: false 53 | Value: true 54 | arm_left_2_link: 55 | Alpha: 1 56 | Show Axes: false 57 | Show Trail: false 58 | Value: true 59 | arm_left_3_link: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | Value: true 64 | arm_left_4_link: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | Value: true 69 | arm_left_5_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | arm_left_6_link: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | arm_left_7_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | arm_left_tool_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | arm_right_1_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | arm_right_2_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | arm_right_3_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | arm_right_4_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | arm_right_5_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | arm_right_6_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | arm_right_7_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | arm_right_tool_link: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | Value: true 127 | base_antenna_left_link: 128 | Alpha: 1 129 | Show Axes: false 130 | Show Trail: false 131 | Value: true 132 | base_antenna_right_link: 133 | Alpha: 1 134 | Show Axes: false 135 | Show Trail: false 136 | Value: true 137 | base_cover_link: 138 | Alpha: 1 139 | Show Axes: false 140 | Show Trail: false 141 | Value: true 142 | base_footprint: 143 | Alpha: 1 144 | Show Axes: false 145 | Show Trail: false 146 | base_imu_link: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | base_laser_link: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | base_link: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | base_mic_back_left_link: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | base_mic_back_right_link: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | base_mic_front_left_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | base_mic_front_right_link: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | base_sonar_01_link: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | base_sonar_02_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | base_sonar_03_link: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | Value: true 191 | caster_back_left_1_link: 192 | Alpha: 1 193 | Show Axes: false 194 | Show Trail: false 195 | Value: true 196 | caster_back_left_2_link: 197 | Alpha: 1 198 | Show Axes: false 199 | Show Trail: false 200 | Value: true 201 | caster_back_right_1_link: 202 | Alpha: 1 203 | Show Axes: false 204 | Show Trail: false 205 | Value: true 206 | caster_back_right_2_link: 207 | Alpha: 1 208 | Show Axes: false 209 | Show Trail: false 210 | Value: true 211 | caster_front_left_1_link: 212 | Alpha: 1 213 | Show Axes: false 214 | Show Trail: false 215 | Value: true 216 | caster_front_left_2_link: 217 | Alpha: 1 218 | Show Axes: false 219 | Show Trail: false 220 | Value: true 221 | caster_front_right_1_link: 222 | Alpha: 1 223 | Show Axes: false 224 | Show Trail: false 225 | Value: true 226 | caster_front_right_2_link: 227 | Alpha: 1 228 | Show Axes: false 229 | Show Trail: false 230 | Value: true 231 | gripper_left_grasping_frame: 232 | Alpha: 1 233 | Show Axes: false 234 | Show Trail: false 235 | gripper_left_left_finger_link: 236 | Alpha: 1 237 | Show Axes: false 238 | Show Trail: false 239 | Value: true 240 | gripper_left_link: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | Value: true 245 | gripper_left_right_finger_link: 246 | Alpha: 1 247 | Show Axes: false 248 | Show Trail: false 249 | Value: true 250 | gripper_left_tool_link: 251 | Alpha: 1 252 | Show Axes: false 253 | Show Trail: false 254 | gripper_right_grasping_frame: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | gripper_right_left_finger_link: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | Value: true 263 | gripper_right_link: 264 | Alpha: 1 265 | Show Axes: false 266 | Show Trail: false 267 | Value: true 268 | gripper_right_right_finger_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | Value: true 273 | gripper_right_tool_link: 274 | Alpha: 1 275 | Show Axes: false 276 | Show Trail: false 277 | head_1_link: 278 | Alpha: 1 279 | Show Axes: false 280 | Show Trail: false 281 | Value: true 282 | head_2_link: 283 | Alpha: 1 284 | Show Axes: false 285 | Show Trail: false 286 | Value: true 287 | rgbd_laser_link: 288 | Alpha: 1 289 | Show Axes: false 290 | Show Trail: false 291 | suspension_left_link: 292 | Alpha: 1 293 | Show Axes: false 294 | Show Trail: false 295 | suspension_right_link: 296 | Alpha: 1 297 | Show Axes: false 298 | Show Trail: false 299 | torso_fixed_column_link: 300 | Alpha: 1 301 | Show Axes: false 302 | Show Trail: false 303 | Value: true 304 | torso_fixed_link: 305 | Alpha: 1 306 | Show Axes: false 307 | Show Trail: false 308 | Value: true 309 | torso_lift_link: 310 | Alpha: 1 311 | Show Axes: false 312 | Show Trail: false 313 | Value: true 314 | wheel_left_link: 315 | Alpha: 1 316 | Show Axes: false 317 | Show Trail: false 318 | Value: true 319 | wheel_right_link: 320 | Alpha: 1 321 | Show Axes: false 322 | Show Trail: false 323 | Value: true 324 | wrist_left_ft_link: 325 | Alpha: 1 326 | Show Axes: false 327 | Show Trail: false 328 | Value: true 329 | wrist_left_ft_tool_link: 330 | Alpha: 1 331 | Show Axes: false 332 | Show Trail: false 333 | Value: true 334 | wrist_right_ft_link: 335 | Alpha: 1 336 | Show Axes: false 337 | Show Trail: false 338 | Value: true 339 | wrist_right_ft_tool_link: 340 | Alpha: 1 341 | Show Axes: false 342 | Show Trail: false 343 | Value: true 344 | xtion_depth_frame: 345 | Alpha: 1 346 | Show Axes: false 347 | Show Trail: false 348 | xtion_depth_optical_frame: 349 | Alpha: 1 350 | Show Axes: false 351 | Show Trail: false 352 | xtion_link: 353 | Alpha: 1 354 | Show Axes: false 355 | Show Trail: false 356 | xtion_optical_frame: 357 | Alpha: 1 358 | Show Axes: false 359 | Show Trail: false 360 | xtion_orbbec_aux_joint_frame: 361 | Alpha: 1 362 | Show Axes: false 363 | Show Trail: false 364 | xtion_rgb_frame: 365 | Alpha: 1 366 | Show Axes: false 367 | Show Trail: false 368 | xtion_rgb_optical_frame: 369 | Alpha: 1 370 | Show Axes: false 371 | Show Trail: false 372 | Name: RobotModel 373 | Robot Description: robot_description 374 | TF Prefix: "" 375 | Update Interval: 0 376 | Value: true 377 | Visual Enabled: true 378 | - Alpha: 0.5 379 | Cell Size: 1 380 | Class: rviz/Grid 381 | Color: 160; 160; 164 382 | Enabled: false 383 | Line Style: 384 | Line Width: 0.029999999329447746 385 | Value: Lines 386 | Name: Grid 387 | Normal Cell Count: 0 388 | Offset: 389 | X: 0 390 | Y: 0 391 | Z: 0 392 | Plane: XY 393 | Plane Cell Count: 10 394 | Reference Frame: 395 | Value: false 396 | - Class: reachability_map_visualizer/ReachabilityMap 397 | Color by Reachability: true 398 | Disect: Full 399 | Enabled: true 400 | Highest Reachability Index: 100 401 | Lowest Reachability Index: 0 402 | Name: ReachabilityMap 403 | Poses Property: 404 | Alpha: 0.20000000298023224 405 | Color: 204; 51; 204 406 | Length: 0.009999999776482582 407 | Shape: Sphere 408 | Shape Property: 409 | Alpha: 0.5 410 | Color: 255; 225; 102 411 | Size: 0.05000000074505806 412 | Show Poses: false 413 | Show Shape: true 414 | Topic: /reachability_map 415 | Unreliable: false 416 | Value: true 417 | - Alpha: 1 418 | Axes Length: 1 419 | Axes Radius: 0.10000000149011612 420 | Class: rviz/Pose 421 | Color: 255; 25; 0 422 | Enabled: true 423 | Head Length: 0.30000001192092896 424 | Head Radius: 0.10000000149011612 425 | Name: Pose 426 | Shaft Length: 1 427 | Shaft Radius: 0.05000000074505806 428 | Shape: Arrow 429 | Topic: "" 430 | Unreliable: false 431 | Value: true 432 | Enabled: true 433 | Global Options: 434 | Background Color: 255; 255; 255 435 | Default Light: true 436 | Fixed Frame: base_footprint 437 | Frame Rate: 30 438 | Name: root 439 | Tools: 440 | - Class: rviz/Interact 441 | Hide Inactive Objects: true 442 | - Class: rviz/MoveCamera 443 | - Class: rviz/Select 444 | - Class: rviz/FocusCamera 445 | - Class: rviz/Measure 446 | - Class: rviz/SetInitialPose 447 | Theta std deviation: 0.2617993950843811 448 | Topic: /initialpose 449 | X std deviation: 0.5 450 | Y std deviation: 0.5 451 | - Class: rviz/SetGoal 452 | Topic: /move_base_simple/goal 453 | - Class: rviz/PublishPoint 454 | Single click: true 455 | Topic: /clicked_point 456 | Value: true 457 | Views: 458 | Current: 459 | Class: rviz/Orbit 460 | Distance: 3.995306968688965 461 | Enable Stereo Rendering: 462 | Stereo Eye Separation: 0.05999999865889549 463 | Stereo Focal Distance: 1 464 | Swap Stereo Eyes: false 465 | Value: false 466 | Focal Point: 467 | X: -0.02300766296684742 468 | Y: 0.5920429825782776 469 | Z: 0.011342588812112808 470 | Focal Shape Fixed Size: true 471 | Focal Shape Size: 0.05000000074505806 472 | Invert Z Axis: false 473 | Name: Current View 474 | Near Clip Distance: 0.009999999776482582 475 | Pitch: 0.3803979754447937 476 | Target Frame: 477 | Value: Orbit (rviz) 478 | Yaw: 0.2022203952074051 479 | Saved: ~ 480 | Window Geometry: 481 | Displays: 482 | collapsed: false 483 | Height: 1043 484 | Hide Left Dock: false 485 | Hide Right Dock: true 486 | QMainWindow State: 000000ff00000000fd0000000400000000000001bc00000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000007800000023100000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005be0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 487 | Selection: 488 | collapsed: false 489 | Time: 490 | collapsed: false 491 | Tool Properties: 492 | collapsed: false 493 | Views: 494 | collapsed: true 495 | Width: 1920 496 | X: 0 497 | Y: 561 498 | -------------------------------------------------------------------------------- /src/hdf5_dataset.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define RANK_OUT 2 4 | 5 | namespace hdf5_dataset 6 | { 7 | 8 | Hdf5Dataset::Hdf5Dataset(std::string fullpath) 9 | { 10 | std::stringstream fp(fullpath); 11 | std::string segment; 12 | std::vector seglist; 13 | char the_path[256]; 14 | 15 | while(std::getline(fp, segment, '/')) 16 | { 17 | seglist.push_back(segment); 18 | } 19 | 20 | std::ostringstream oss_file; 21 | oss_file<< seglist.back(); 22 | this->filename_ = oss_file.str(); 23 | 24 | 25 | seglist.pop_back(); 26 | std::ostringstream oss_path; 27 | if (!seglist.empty()) 28 | { 29 | std::copy(seglist.begin(), seglist.end()-1, 30 | std::ostream_iterator(oss_path, "/")); 31 | oss_path << seglist.back(); 32 | oss_path<<"/"; 33 | } 34 | else 35 | { 36 | getcwd(the_path, 255); 37 | strcat(the_path, "/"); 38 | oss_path << the_path; 39 | } 40 | 41 | this->path_ = oss_path.str(); 42 | checkPath(this->path_); 43 | checkFileName(this->filename_); 44 | } 45 | 46 | Hdf5Dataset::Hdf5Dataset(std::string path, std::string filename) 47 | { 48 | this->path_ = path; 49 | this->filename_ = filename; 50 | checkPath(this->path_); 51 | checkFileName(this->filename_); 52 | } 53 | 54 | bool Hdf5Dataset::open() 55 | { 56 | std::string fullpath = this->path_ + this->filename_; 57 | ROS_INFO("Opening map %s", fullpath.c_str()); 58 | this->file_ = H5Fopen(fullpath.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT); 59 | 60 | this->group_poses_ = H5Gopen(this->file_, "/Poses", H5P_DEFAULT); 61 | this->poses_dataset_ = H5Dopen(this->group_poses_, "poses_dataset", H5P_DEFAULT); 62 | 63 | this->group_spheres_ = H5Gopen(this->file_, "/Spheres", H5P_DEFAULT); 64 | this->sphere_dataset_ = H5Dopen(this->group_spheres_, "sphere_dataset", H5P_DEFAULT); 65 | 66 | this->attr_ = H5Aopen(this->sphere_dataset_, "Resolution", H5P_DEFAULT); 67 | herr_t ret = H5Aread(this->attr_, H5T_NATIVE_FLOAT, &this->res_); 68 | 69 | } 70 | 71 | void Hdf5Dataset::close() 72 | { 73 | H5Aclose(this->attr_); 74 | H5Dclose(this->poses_dataset_); 75 | H5Gclose(this->group_poses_); 76 | H5Dclose(this->sphere_dataset_); 77 | H5Gclose(this->group_spheres_); 78 | H5Fclose(this->file_); 79 | } 80 | 81 | 82 | bool Hdf5Dataset::checkPath(std::string path) 83 | { 84 | if (stat(path.c_str(), &st)!=0) 85 | { 86 | ROS_INFO("Path does not exist yet"); 87 | //return false; 88 | } 89 | } 90 | 91 | void Hdf5Dataset::createPath(std::string path) 92 | { 93 | ROS_INFO("Creating Directory"); 94 | const int dir_err = mkdir(this->path_.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); 95 | if(1 == dir_err) 96 | { 97 | ROS_INFO("Error Creating Directory"); 98 | exit(1); 99 | } 100 | } 101 | 102 | bool Hdf5Dataset::checkFileName(std::string filename) 103 | { 104 | std::string ext = ".h5"; 105 | if(filename.find(ext) == std::string::npos) 106 | { 107 | ROS_ERROR("Please provide an extension of .h5 It will make life easy"); 108 | exit(1); 109 | } 110 | } 111 | 112 | bool Hdf5Dataset::saveReachMapsToDataset( MultiMapPtr& poses, MapVecDoublePtr& spheres, float resolution) 113 | { 114 | if(!checkPath(this->path_)) 115 | { 116 | createPath(this->path_); 117 | } 118 | 119 | //Creating Multimap to Straight vector > //Can take this function to a new class 120 | std::vector< std::vector< double > > pose_reach; 121 | for (MultiMapPtr::iterator it = poses.begin(); it != poses.end(); ++it) 122 | { 123 | const std::vector* sphere_coord = it->first; 124 | const std::vector* point_on_sphere = it->second; 125 | std::vector< double > pose_and_sphere(10); 126 | //pose_and_sphere.reserve( sphere_coord->size() + point_on_sphere->size()); 127 | for (int i = 0; i < 3; i++) 128 | { 129 | pose_and_sphere[i]=((*sphere_coord)[i]); 130 | } 131 | for (int j = 0; j < 7; j++) 132 | { 133 | pose_and_sphere[3+j] = ((*point_on_sphere)[j]); 134 | } 135 | pose_reach.push_back(pose_and_sphere); 136 | } 137 | 138 | std::string fullpath = this->path_ + this->filename_; 139 | ROS_INFO("Saving map %s", fullpath.c_str()); 140 | 141 | this->file_ = H5Fcreate(fullpath.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT); 142 | this->group_poses_ = H5Gcreate(this->file_, "/Poses", H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT); 143 | this->group_spheres_ = H5Gcreate(this->file_, "/Spheres", H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT); 144 | ROS_INFO("Saving poses in reachability map"); 145 | const hsize_t ndims = 2; 146 | const hsize_t ncols = 10; 147 | 148 | int posSize = poses.size(); 149 | int chunk_size; 150 | int PY = 10; 151 | if (posSize % 2) 152 | { 153 | chunk_size = (posSize / 2) + 1; 154 | } 155 | else 156 | { 157 | chunk_size = (posSize / 2); 158 | } 159 | // Create Dataspace 160 | hsize_t dims[ndims] = {0, ncols}; // Starting with an empty buffer 161 | hsize_t max_dims[ndims] = {H5S_UNLIMITED, ncols}; // Creating dataspace 162 | hid_t file_space = H5Screate_simple(ndims, dims, max_dims); 163 | 164 | // Create Dataset Property list 165 | hid_t plist = H5Pcreate(H5P_DATASET_CREATE); 166 | H5Pset_layout(plist, H5D_CHUNKED); 167 | hsize_t chunk_dims[ndims] = {chunk_size, ncols}; 168 | H5Pset_chunk(plist, ndims, chunk_dims); 169 | 170 | 171 | // Create the datset 172 | this->poses_dataset_ = H5Dcreate(this->group_poses_, "poses_dataset", H5T_NATIVE_FLOAT, file_space, H5P_DEFAULT, plist, H5P_DEFAULT); 173 | // Closing resources 174 | H5Pclose(plist); 175 | H5Sclose(file_space); 176 | 177 | // Creating the first buffer 178 | hsize_t nlines = chunk_size; 179 | float *buffer = new float[nlines * ncols]; 180 | float **dset1_data = new float *[nlines]; 181 | for (hsize_t i = 0; i < nlines; ++i) 182 | { 183 | dset1_data[i] = &buffer[i * ncols]; 184 | } 185 | 186 | // Data for the first chunk 187 | for (int i = 0; i < chunk_size; i++) 188 | { 189 | for (int j = 0; j < PY; j++) 190 | { 191 | dset1_data[i][j] = pose_reach[i][j]; 192 | } 193 | } 194 | // Memory dataspace indicating size of the buffer 195 | dims[0] = chunk_size; 196 | dims[1] = ncols; 197 | hid_t mem_space = H5Screate_simple(ndims, dims, NULL); 198 | 199 | // Extending dataset 200 | dims[0] = chunk_size; 201 | dims[1] = ncols; 202 | H5Dset_extent(this->poses_dataset_, dims); 203 | 204 | // Selecting hyperslab on the dataset 205 | file_space = H5Dget_space(this->poses_dataset_); 206 | hsize_t start[2] = {0, 0}; 207 | hsize_t count[2] = {chunk_size, ncols}; 208 | H5Sselect_hyperslab(file_space, H5S_SELECT_SET, start, NULL, count, NULL); 209 | 210 | // Writing buffer to the dataset 211 | H5Dwrite(this->poses_dataset_, H5T_NATIVE_FLOAT, mem_space, file_space, H5P_DEFAULT, buffer); 212 | 213 | // Closing file dataspace 214 | H5Sclose(file_space); 215 | // Data for the Second chunk 216 | for (int i = chunk_size; i < posSize; i++) 217 | { 218 | for (int j = 0; j < PY; j++) 219 | { 220 | dset1_data[i - chunk_size][j] = pose_reach[i][j]; 221 | } 222 | } 223 | 224 | // Resizing new memory dataspace indicating new size of the buffer 225 | dims[0] = posSize - chunk_size; 226 | dims[1] = ncols; 227 | H5Sset_extent_simple(mem_space, ndims, dims, NULL); 228 | 229 | // Extend dataset 230 | dims[0] = posSize; 231 | dims[1] = ncols; 232 | H5Dset_extent(this->poses_dataset_, dims); 233 | // Selecting hyperslab 234 | file_space = H5Dget_space(this->poses_dataset_); 235 | start[0] = chunk_size; 236 | start[1] = 0; 237 | count[0] = posSize - chunk_size; 238 | count[1] = ncols; 239 | H5Sselect_hyperslab(file_space, H5S_SELECT_SET, start, NULL, count, NULL); 240 | 241 | // Writing buffer to dataset 242 | H5Dwrite(this->poses_dataset_, H5T_NATIVE_FLOAT, mem_space, file_space, H5P_DEFAULT, buffer); 243 | 244 | // Closing all the resources 245 | delete[] dset1_data; 246 | delete[] buffer; 247 | 248 | 249 | // Creating Sphere dataset 250 | ROS_INFO("Saving spheres in Reachability map"); 251 | hid_t sphere_dataspace; 252 | const int SX = spheres.size(); 253 | const int SY = 4; 254 | 255 | hsize_t dims2[2]; // dataset dimensions 256 | dims2[0] = SX; 257 | dims2[1] = SY; 258 | double dset2_data[SX][SY]; 259 | 260 | for (MapVecDoublePtr::iterator it = spheres.begin(); it !=spheres.end(); ++it) 261 | { 262 | for (int j = 0; j < SY - 1; j++) 263 | { 264 | dset2_data[distance( spheres.begin(), it)][j] = (*it->first)[j]; 265 | } 266 | for (int j = 3; j < SY; j++) 267 | { 268 | dset2_data[distance( spheres.begin(), it)][j] = it->second; 269 | } 270 | } 271 | sphere_dataspace = H5Screate_simple(2, dims2, NULL); 272 | this->sphere_dataset_ = H5Dcreate2(this->group_spheres_, "sphere_dataset", H5T_NATIVE_DOUBLE, 273 | sphere_dataspace, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT); 274 | H5Dwrite(this->sphere_dataset_, H5T_NATIVE_DOUBLE, H5S_ALL, H5S_ALL, H5P_DEFAULT, dset2_data); 275 | 276 | // Creating attribute 277 | 278 | 279 | hsize_t attr_dims; 280 | float attr_data[1]; 281 | attr_data[0] = resolution; 282 | attr_dims = 1; 283 | sphere_dataspace = H5Screate_simple(1, &attr_dims, NULL); 284 | this->attr_ = H5Acreate2(this->sphere_dataset_, "Resolution", H5T_NATIVE_FLOAT, sphere_dataspace, 285 | H5P_DEFAULT, H5P_DEFAULT); 286 | H5Awrite(this->attr_, H5T_NATIVE_FLOAT, attr_data); 287 | //H5Aclose(this->attr_); 288 | 289 | // Closing all 290 | 291 | H5Sclose(sphere_dataspace); 292 | H5Sclose(file_space); 293 | H5Sclose(mem_space); 294 | close(); 295 | } 296 | 297 | 298 | bool Hdf5Dataset::h5ToMultiMapPosesAndSpheres(MultiMapPtr& pose_col, MapVecDoublePtr& sphere_col) 299 | { 300 | //The process of creting typedef MultiMap is little bit tricky. 301 | //As the data are read from the h5 file as chuck of array, the retrieved sphere addresses comes different for every sphere 302 | //But that is not the case for our map structure. Every sphere with same coordinate has same address in h5 303 | //So to create the exact same Multimap and Map while maintaining the same strcture, we have to assign the spheres that have the same coordinates, with the same address 304 | //Otherwise when we will be loading the dataset for visualization or other taks such as inverse map, the comparison between the coordinates of sphere dataset and 305 | //coordinates of spheres in Poses dataset are going to fail. 306 | 307 | MultiMap sphere_and_poses; 308 | MapVecDouble sps; 309 | h5ToMultiMapPoses(sphere_and_poses, sps); 310 | 311 | MapVecDouble sp_col; 312 | h5ToMultiMapSpheres(sp_col); 313 | 314 | for(MapVecDouble::iterator it=sps.begin(); it!=sps.end();++it) 315 | { 316 | //const std::vector* sp = &(it->first); 317 | std::vector* sp = new std::vector(3); 318 | (*sp)[0] = (it->first)[0]; 319 | (*sp)[1] = (it->first)[1]; 320 | (*sp)[2] = (it->first)[2]; 321 | MultiMap::iterator it1; 322 | for( it1= sphere_and_poses.lower_bound(it->first); it1 != sphere_and_poses.upper_bound(it->first); ++it1) 323 | { 324 | std::vector* ps = new std::vector(7); 325 | (*ps)[0]=(it1->second[0]); 326 | (*ps)[1]=(it1->second[1]); 327 | (*ps)[2]=(it1->second[2]); 328 | (*ps)[3]=(it1->second[3]); 329 | (*ps)[4]=(it1->second[4]); 330 | (*ps)[5]=(it1->second[5]); 331 | (*ps)[6]=(it1->second[6]); 332 | pose_col.insert(std::make_pair(sp, ps)); 333 | } 334 | 335 | for (MapVecDouble::iterator it2 = sp_col.lower_bound(it->first); it2 !=sp_col.upper_bound(it->first); ++it2) 336 | { 337 | sphere_col.insert(std::make_pair(sp, it2->second)); 338 | } 339 | } 340 | return 0; 341 | } 342 | 343 | bool Hdf5Dataset::h5ToMultiMapPoses(MultiMap& pose_col) 344 | { 345 | MapVecDouble sphere_col; 346 | h5ToMultiMapPoses(pose_col, sphere_col); 347 | } 348 | 349 | bool Hdf5Dataset::h5ToMultiMapPoses(MultiMap& pose_col, MapVecDouble& sphere_col) 350 | { 351 | hsize_t dims_out[2], count[2], offset[2]; 352 | hid_t dataspace = H5Dget_space(this->poses_dataset_); /* dataspace handle */ 353 | int rank = H5Sget_simple_extent_ndims(dataspace); 354 | herr_t status_n = H5Sget_simple_extent_dims(dataspace, dims_out, NULL); 355 | herr_t status; 356 | int chunk_size, chunk_itr; 357 | if (dims_out[0] % 10) 358 | { 359 | chunk_itr = 11; 360 | } 361 | else 362 | { 363 | chunk_itr = 10; 364 | } 365 | chunk_size = (dims_out[0] / 10); 366 | offset[0] = 0; 367 | 368 | for (int it = 0; it < chunk_itr; it++) 369 | { 370 | offset[1] = 0; 371 | if ((dims_out[0] - (chunk_size * it)) / chunk_size != 0) 372 | { 373 | count[0] = chunk_size; 374 | offset[0] = chunk_size * it; 375 | } 376 | else 377 | { 378 | count[0] = (dims_out[0] - (chunk_size * it)); 379 | offset[0] = count[0]; 380 | } 381 | count[1] = 10; 382 | 383 | double data_out[count[0]][count[1]]; 384 | 385 | status = H5Sselect_hyperslab(dataspace, H5S_SELECT_SET, offset, NULL, count, NULL); 386 | hsize_t dimsm[2]; 387 | dimsm[0] = count[0]; 388 | dimsm[1] = count[1]; 389 | hid_t memspace; 390 | memspace = H5Screate_simple(RANK_OUT, dimsm, NULL); 391 | status = H5Dread(this->poses_dataset_, H5T_NATIVE_DOUBLE, memspace, dataspace, H5P_DEFAULT, data_out); 392 | 393 | for(int i=0; i sphere_center(3); 396 | std::vector Poses(7); 397 | for(int j=0; j<3;j++) 398 | { 399 | sphere_center[j] = data_out[i][j]; 400 | } 401 | for(int k=3;k<10; k++) 402 | { 403 | Poses[k-3] = data_out[i][k]; 404 | } 405 | pose_col.insert(std::make_pair(sphere_center, Poses)); 406 | sphere_col.insert(std::make_pair(sphere_center, double(i))); 407 | } 408 | } 409 | return 0; 410 | } 411 | 412 | bool Hdf5Dataset::h5ToMultiMapSpheres(MapVecDouble& sphere_col) 413 | { 414 | hsize_t dims_out[2], count[2], offset[2], dimsm[2]; 415 | hid_t dataspace = H5Dget_space(this->sphere_dataset_); // dataspace handle 416 | int rank = H5Sget_simple_extent_ndims(dataspace); 417 | herr_t status_n = H5Sget_simple_extent_dims(dataspace, dims_out, NULL); 418 | herr_t status; 419 | offset[0] = 0; 420 | offset[1] = 0; 421 | count[0] = dims_out[0]; 422 | count[1] = 4; 423 | double data_out[count[0]][count[1]]; 424 | status = H5Sselect_hyperslab(dataspace, H5S_SELECT_SET, offset, NULL, count, NULL); 425 | dimsm[0] = count[0]; 426 | dimsm[1] = count[1]; 427 | hid_t memspace; 428 | memspace = H5Screate_simple(RANK_OUT, dimsm, NULL); 429 | status = H5Dread(this->sphere_dataset_, H5T_NATIVE_DOUBLE, memspace, dataspace, H5P_DEFAULT, data_out); 430 | for (int i = 0; i < count[0]; i++) 431 | { 432 | std::vector< double > sphere_center(3); 433 | double ri; 434 | for (int j = 0; j < 3; j++) 435 | { 436 | sphere_center[j] = data_out[i][j]; 437 | } 438 | for (int k = 3; k < 4; k++) 439 | { 440 | ri = data_out[i][k]; 441 | } 442 | sphere_col.insert(std::pair< std::vector< double >, double >(sphere_center, ri)); 443 | } 444 | return 0; 445 | } 446 | 447 | bool Hdf5Dataset::h5ToResolution(float &resolution) 448 | { 449 | resolution = this->res_; 450 | return 0; 451 | } 452 | 453 | 454 | bool Hdf5Dataset::loadMapsFromDataset(MultiMapPtr& poses, MapVecDoublePtr& spheres, float &resolution) 455 | { 456 | h5ToMultiMapPosesAndSpheres(poses, spheres); 457 | h5ToResolution(resolution); 458 | close(); 459 | return 0; 460 | } 461 | 462 | bool Hdf5Dataset::loadMapsFromDataset(MultiMapPtr& poses, MapVecDoublePtr& spheres) 463 | { 464 | h5ToMultiMapPosesAndSpheres(poses, spheres); 465 | close(); 466 | return 0; 467 | } 468 | 469 | 470 | bool Hdf5Dataset::loadMapsFromDataset(MultiMap& poses, MapVecDouble& spheres) 471 | { 472 | h5ToMultiMapPoses(poses); 473 | h5ToMultiMapSpheres(spheres); 474 | close(); 475 | return 0; 476 | } 477 | 478 | bool Hdf5Dataset::loadMapsFromDataset(MultiMap& poses, MapVecDouble& spheres, float &resolution) 479 | { 480 | h5ToMultiMapPoses(poses); 481 | h5ToMultiMapSpheres(spheres); 482 | h5ToResolution(resolution); 483 | close(); 484 | return 0; 485 | } 486 | 487 | 488 | }; 489 | --------------------------------------------------------------------------------