├── map_creator ├── msg │ ├── capability.msg │ ├── WorkSpace.msg │ ├── capShape.msg │ └── WsSphere.msg ├── maps │ ├── motoman_mh5_r1_capability.h5 │ ├── motoman_mh5_r0.08_reachability.h5 │ └── motoman_mh5_r0.08_Inv_reachability.h5 ├── include │ └── map_creator │ │ ├── kinematics.h │ │ ├── hdf5_dataset.h │ │ └── sphere_discretization.h ├── src │ ├── load_capability_map.cpp │ ├── load_reachability_map.cpp │ ├── create_reachability_map.cpp │ ├── create_inverse_reachability_map.cpp │ ├── kinematics.cpp │ └── create_capability_map.cpp ├── package.xml ├── CMakeLists.txt ├── test │ └── utest.cpp ├── README.md └── LICENSE ├── reuleaux ├── CMakeLists.txt └── package.xml ├── workspace_visualization ├── icons │ └── classes │ │ ├── CapabilityMap.png │ │ └── ReachabilityMap.png ├── plugin_description.xml ├── src │ ├── capability_map_visual.h │ ├── reachability_map_visual.h │ ├── capability_map_display.h │ ├── reachability_map_display.h │ ├── capability_map_display.cpp │ ├── capability_map_visual.cpp │ ├── reachability_map_display.cpp │ └── reachability_map_visual.cpp ├── README.md ├── CMakeLists.txt ├── package.xml └── LICENSE ├── base_placement_plugin ├── launch │ ├── moveit_pkgs.launch │ └── base_placement.launch ├── plugin.xml ├── scripts │ └── helper.bash ├── include │ └── base_placement_plugin │ │ ├── point_tree_item.h │ │ ├── point_tree_model.h │ │ ├── create_marker.h │ │ ├── add_robot_base.h │ │ ├── add_way_point.h │ │ ├── widgets │ │ └── base_placement_widget.h │ │ └── place_base.h ├── README.md ├── src │ ├── point_tree_item.cpp │ ├── point_tree_model.cpp │ └── create_marker.cpp ├── CMakeLists.txt ├── package.xml ├── rviz │ └── base_placement.rviz └── LICENSE ├── .gitignore ├── .travis.yml └── README.md /map_creator/msg/capability.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float32 resolution 4 | 5 | map_creator/capShape[] capShapes 6 | -------------------------------------------------------------------------------- /map_creator/msg/WorkSpace.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | map_creator/WsSphere[] WsSpheres 4 | 5 | float32 resolution 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /reuleaux/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(reuleaux) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /map_creator/maps/motoman_mh5_r1_capability.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/reuleaux/HEAD/map_creator/maps/motoman_mh5_r1_capability.h5 -------------------------------------------------------------------------------- /map_creator/maps/motoman_mh5_r0.08_reachability.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/reuleaux/HEAD/map_creator/maps/motoman_mh5_r0.08_reachability.h5 -------------------------------------------------------------------------------- /map_creator/maps/motoman_mh5_r0.08_Inv_reachability.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/reuleaux/HEAD/map_creator/maps/motoman_mh5_r0.08_Inv_reachability.h5 -------------------------------------------------------------------------------- /map_creator/msg/capShape.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | float32 identifier 4 | 5 | float32 ri 6 | 7 | float32 angleSFE 8 | 9 | geometry_msgs/Pose pose 10 | 11 | -------------------------------------------------------------------------------- /workspace_visualization/icons/classes/CapabilityMap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/reuleaux/HEAD/workspace_visualization/icons/classes/CapabilityMap.png -------------------------------------------------------------------------------- /map_creator/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 | -------------------------------------------------------------------------------- /workspace_visualization/icons/classes/ReachabilityMap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-industrial-attic/reuleaux/HEAD/workspace_visualization/icons/classes/ReachabilityMap.png -------------------------------------------------------------------------------- /base_placement_plugin/launch/moveit_pkgs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /base_placement_plugin/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Tool for visualizing base placement methods 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /base_placement_plugin/scripts/helper.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ "$1" != "" ]; then 4 | echo "hello" 5 | echo "$PWD/$1" 6 | 7 | 8 | roslaunch "$1" planning_context.launch load_robot_description:=true 9 | roslaunch "$1" move_group.launch allow_trajectory_execution:=true fake_execution:=true info:=true 10 | else 11 | echo "$0: Please provide a moveit package for your robot:" 12 | fi 13 | -------------------------------------------------------------------------------- /base_placement_plugin/launch/base_placement.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | -------------------------------------------------------------------------------- /workspace_visualization/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Displays poses and spheres of reachability map 7 | 8 | map_creator::WorkSpace 9 | 10 | 11 | 12 | 15 | 16 | Displays shapes of capability map 17 | 18 | map_creator::capability 19 | 20 | 21 | -------------------------------------------------------------------------------- /reuleaux/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | reuleaux 4 | 0.0.0 5 | reuleaux metapackage 6 | Abhijit Makhal 7 | Abhijit Makhal 8 | Apache 2.0 9 | http://wiki.ros.org/reuleaux 10 | https://github.com/ros-industrial-consortium/reuleaux 11 | https://github.com/ros-industrial-consortium/reuleaux/issues 12 | 13 | catkin 14 | base_placement_plugin 15 | map_creator 16 | workspace_visualization 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/point_tree_item.h: -------------------------------------------------------------------------------- 1 | #ifndef PointTreeItem_H 2 | #define PointTreeItem_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class PointTreeItem 9 | { 10 | public: 11 | PointTreeItem(const QVector< QVariant > &data, PointTreeItem *parent = 0); 12 | ~PointTreeItem(); 13 | 14 | PointTreeItem *child(int number); 15 | int childCount() const; 16 | int columnCount() const; 17 | QVariant data(int column) const; 18 | bool insertChildren(int position, int count, int columns); 19 | bool insertColumns(int position, int columns); 20 | PointTreeItem *parent(); 21 | bool removeChildren(int position, int count); 22 | bool removeColumns(int position, int columns); 23 | int childNumber() const; 24 | bool setData(int column, const QVariant &value); 25 | 26 | private: 27 | QList< PointTreeItem * > childItems; 28 | QVector< QVariant > itemData; 29 | PointTreeItem *parentItem; 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /workspace_visualization/src/capability_map_visual.h: -------------------------------------------------------------------------------- 1 | #ifndef CAPABILITY_MAP_VISUAL_H 2 | #define CAPABILITY_MAP_VISUAL_H 3 | 4 | #include 5 | 6 | namespace Ogre 7 | { 8 | class Vector3; 9 | class Quaternion; 10 | } 11 | 12 | namespace rviz 13 | { 14 | class Shape; 15 | } 16 | 17 | namespace workspace_visualization 18 | { 19 | class CapMapDisplay; 20 | class CapMapVisual 21 | { 22 | public: 23 | CapMapVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, rviz::DisplayContext* display); 24 | virtual ~CapMapVisual(); 25 | void setMessage(const map_creator::capability::ConstPtr& msg, int low_ri, int high_ri, int disect_choice); 26 | void setFramePosition(const Ogre::Vector3& position); 27 | void setFrameOrientation(const Ogre::Quaternion& orientation); 28 | 29 | void setColor(float r, float g, float b, float a); 30 | void setSize(float l); 31 | 32 | void setColorbyRI(float alpha); 33 | 34 | private: 35 | std::vector< boost::shared_ptr< rviz::Shape > > sphere_; 36 | std::vector< int > colorRI_; 37 | 38 | Ogre::SceneNode* frame_node_; 39 | 40 | Ogre::SceneManager* scene_manager_; 41 | }; 42 | } // end namespace workspace_visualization 43 | #endif // CAPABILITY_MAP_VISUAL_H 44 | -------------------------------------------------------------------------------- /map_creator/include/map_creator/kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef KINEMATICS 2 | #define IKFAST_HAS_LIBRARY // Build IKFast with API functions 3 | #define IKFAST_NO_MAIN 4 | 5 | #define IK_VERSION 61 6 | #include "mh5_ikfast.cpp" 7 | //#include "abb_irb2400_manipulator_ikfast_solver.cpp" 8 | //#include "ur5_ikfast.cpp" 9 | 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #if IK_VERSION > 54 22 | #define IKREAL_TYPE IkReal // for IKFast 56,61 23 | #else 24 | #define IKREAL_TYPE IKReal // for IKFast 54 25 | #endif 26 | 27 | namespace kinematics 28 | { 29 | class Kinematics 30 | { 31 | public: 32 | // kinematics(); 33 | //~kinematics(); 34 | 35 | float SIGN(float x); 36 | float NORM(float a, float b, float c, float d); 37 | void getPoseFromFK(const std::vector< double > joint_values, std::vector< double >& pose); 38 | 39 | bool isIKSuccess(const std::vector &pose, std::vector &joints, int& numOfSolns); 40 | 41 | 42 | 43 | const std::string getRobotName(); 44 | 45 | bool isIkSuccesswithTransformedBase(const geometry_msgs::Pose& base_pose, const geometry_msgs::Pose& grasp_pose, std::vector& joint_soln, 46 | int& numOfSolns); 47 | 48 | 49 | }; 50 | } 51 | 52 | #endif // KINEMATICS 53 | -------------------------------------------------------------------------------- /workspace_visualization/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 workspace_visualization 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 map_creator::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 workspace_visualization 48 | #endif // ReachMap_VISUAL_H 49 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | notifications: 7 | email: 8 | on_success: always 9 | on_failure: always 10 | # recipients: 11 | # - jane@doe 12 | env: 13 | matrix: 14 | - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 15 | - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 16 | - USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 17 | - USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 18 | - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 19 | - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 20 | matrix: 21 | allow_failures: 22 | - env: USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 23 | - env: USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 24 | - env: USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 25 | - env: USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu 26 | install: 27 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 28 | script: 29 | - .ci_config/travis.sh 30 | # - source ./travis.sh # Enable this when you have a package-local script 31 | -------------------------------------------------------------------------------- /workspace_visualization/src/capability_map_display.h: -------------------------------------------------------------------------------- 1 | #ifndef CAPABILITY_MAP_DISPLAY_H 2 | #define CAPABILITY_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 workspace_visualization 26 | { 27 | class CapMapVisual; 28 | class CapMapDisplay : public rviz::MessageFilterDisplay< map_creator::capability > 29 | { 30 | Q_OBJECT 31 | public: 32 | enum Disect 33 | { 34 | Full, 35 | First_Half, 36 | Second_Half, 37 | Middle_Slice, 38 | End_Slice, 39 | }; 40 | CapMapDisplay(); 41 | virtual ~CapMapDisplay(); 42 | 43 | protected: 44 | virtual void onInitialize(); 45 | virtual void reset(); 46 | 47 | private Q_SLOTS: 48 | void updateColorAndAlpha(); 49 | void updateSize(); 50 | 51 | private: 52 | void processMessage(const map_creator::capability::ConstPtr& msg); 53 | std::vector< boost::shared_ptr< CapMapVisual > > visuals_; 54 | 55 | rviz::ColorProperty* color_property_; 56 | rviz::FloatProperty* alpha_property_; 57 | rviz::FloatProperty* radius_property_; 58 | rviz::IntProperty* lower_bound_reachability_; 59 | rviz::IntProperty* upper_bound_reachability_; 60 | rviz::BoolProperty* is_byReachability_; 61 | rviz::EnumProperty* disect_property_; 62 | }; 63 | 64 | } // end namespace workspace_visualization 65 | #endif // CAPABILITY_MAP_DISPLAY_H 66 | -------------------------------------------------------------------------------- /workspace_visualization/README.md: -------------------------------------------------------------------------------- 1 | ## Workspace Visualization 2 | ==== 3 | It is relatively very easy to visualize the map created in map_creator package. Please run rviz first in a terminal. 4 | 5 | rosrun rviz rviz 6 | 7 | Go to add new display, add the display panel of your desired map (reachability/ capability) and set the topic to /reachability_map or /capability_map 8 | In a second terminal load the map (load any one map at a single time, otherwise it may lock up the system) 9 | To load reachability map: 10 | 11 | rosrun map_creator load_reachability_map motorman_mh5_r0.08_rechability.h5 12 | 13 | To load capability map: 14 | 15 | rosrun map_creator load_capability_map motorman_mh5_r0.08_capability.h5 16 | 17 | You can also visualize your inverse reachability map, by the same process as reachability map. Just provide the name of your inverse reachability map name instead of reachability map. 18 | The map would look like mostly a sphere with color variation based on reachability index. Though the inverse reachability map has an uneven shape as it is the transformation of the reachability map. 19 | The blue spheres are the most reachable upto red for less reachability. The red spheres are mostly located at the outer space of the workspace. The size of the spheres can be altered by changing the values in the display. The default shape is sphere. It can also be changed to cone, cylinder and boxes. The workspace can be cut in halves from the display panel. The reachable poses can also be visualized by checking the checkbox in the display. 20 | For a more detailed description of the features of workspace_visualization display panel, please refer to [http://wiki.ros.org/reuleaux] (http://wiki.ros.org/reuleaux) 21 | -------------------------------------------------------------------------------- /workspace_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(workspace_visualization) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rviz 6 | map_creator 7 | ) 8 | catkin_package(INCLUDE_DIRS ) 9 | include_directories(include ${catkin_INCLUDE_DIRS}) 10 | link_directories(${catkin_LIBRARY_DIRS}) 11 | 12 | set(CMAKE_AUTOMOC ON) 13 | 14 | if(rviz_QT_VERSION VERSION_LESS "5") 15 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 16 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 17 | ## pull in all required include dirs, define QT_LIBRARIES, etc. 18 | include(${QT_USE_FILE}) 19 | else() 20 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 21 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 22 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 23 | set(QT_LIBRARIES Qt5::Widgets) 24 | endif() 25 | 26 | add_definitions(-DQT_NO_KEYWORDS) 27 | 28 | set(SRC_FILES 29 | src/reachability_map_visual.cpp 30 | src/reachability_map_display.cpp 31 | src/capability_map_visual.cpp 32 | src/capability_map_display.cpp 33 | 34 | ) 35 | 36 | add_library(${PROJECT_NAME} ${SRC_FILES}) 37 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 38 | 39 | install(TARGETS 40 | ${PROJECT_NAME} 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | install(FILES 47 | plugin_description.xml 48 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 49 | 50 | 51 | install(DIRECTORY icons/ 52 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) 53 | 54 | 55 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/point_tree_model.h: -------------------------------------------------------------------------------- 1 | #ifndef PointTreeModel_H 2 | #define PointTreeModel_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class PointTreeItem; 9 | 10 | class PointTreeModel : public QAbstractItemModel 11 | { 12 | Q_OBJECT 13 | 14 | public: 15 | PointTreeModel(const QStringList &headers, const QString &data, QObject *parent = 0); 16 | ~PointTreeModel(); 17 | QVariant data(const QModelIndex &index, int role) const; 18 | QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const; 19 | 20 | QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const; 21 | QModelIndex parent(const QModelIndex &index) const; 22 | 23 | int rowCount(const QModelIndex &parent = QModelIndex()) const; 24 | int columnCount(const QModelIndex &parent = QModelIndex()) const; 25 | 26 | Qt::ItemFlags flags(const QModelIndex &index) const; 27 | bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole); 28 | 29 | bool setHeaderData(int section, Qt::Orientation orientation, const QVariant &value, int role = Qt::EditRole); 30 | 31 | bool insertColumns(int position, int columns, const QModelIndex &parent = QModelIndex()); 32 | bool removeColumns(int position, int columns, const QModelIndex &parent = QModelIndex()); 33 | bool insertRows(int position, int rows, const QModelIndex &parent = QModelIndex()); 34 | bool removeRows(int position, int rows, const QModelIndex &parent = QModelIndex()); 35 | 36 | private: 37 | void setupModelData(const QStringList &lines, PointTreeItem *parent); 38 | PointTreeItem *getItem(const QModelIndex &index) const; 39 | 40 | PointTreeItem *rootItem; 41 | }; 42 | 43 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # reuleaux 2 | ===== 3 | [![Build Status](https://travis-ci.com/ros-industrial-consortium/reuleaux.svg?branch=master)](https://travis-ci.com/ros-industrial-consortium/reuleaux) 4 | 5 | Google Summer of Code Project: Robot reachability and Base Placement 6 | 7 | 8 | [Franz Reuleaux] (https://en.wikipedia.org/wiki/Franz_Reuleaux): *Franz Reuleaux (30 September 1829 -20 August 1905) was a lecturer of the Berlin Royal Technical Academy where he was later appointed as the President. He was often referred as the “father of kinematics”. After becoming a professor at the Swiss Federal Institute of Zurich, he became Rector at Königs Technischen Hochschule Berlin – Charlottenburg. Despite his involvement in German politics he became widely-known as an engineer-scientist, professor and industrial consultant, education reformer and leader of technical elite of Germany. He was also a member of Royal Swiss Acedemy of Sciences. 9 | Reuleaux relied deeply on the concept of kinematic chain which could be abstracted in kinematic pairs- chains of elementary links. Constraints on each kinematic pairs can lead to constraints on whole machine. He is best remembered for Reuleaux triangle and the development of compact symbolic notion to describe the topology of very wide variety of mechanisms.* 10 | 11 | 12 | 13 | Documentation about Map creation can be found at: [map_creator] (https://github.com/ros-industrial-consortium/reuleaux/tree/master/map_creator) 14 | 15 | Documentation about Workspace Visualization can be found at: [workspace_visualization] (https://github.com/ros-industrial-consortium/reuleaux/tree/master/workspace_visualization) 16 | 17 | Documentation about Base Placement Planner can be found at: [Base Placement Plugin] (https://github.com/ros-industrial-consortium/reuleaux/tree/master/base_placement_plugin) 18 | 19 | Please refer to [http://wiki.ros.org/reuleaux] (http://wiki.ros.org/reuleaux) for detailed description and instructions. 20 | -------------------------------------------------------------------------------- /map_creator/src/load_capability_map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "map_creator/capability.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 Capability map. If you have not created it yet, Please create the " 10 | "map by running the create cpability map launch file in map_creator"); 11 | return 1; 12 | } 13 | else 14 | { 15 | ros::init(argc, argv, "workspace"); 16 | ros::NodeHandle n; 17 | bool latchOn = 1; 18 | ros::Publisher workspace_pub = n.advertise< map_creator::capability >("capability_map", 1); 19 | 20 | ros::Rate loop_rate(10); 21 | int count = 0; 22 | 23 | hdf5_dataset::Hdf5Dataset h5(argv[1]); 24 | h5.open_cap(); 25 | 26 | VectorOfVectors cap_data; 27 | float resolution; 28 | h5.loadCapMapFromDataset(cap_data, resolution); 29 | 30 | // Creating messages 31 | 32 | map_creator::capability cp; 33 | cp.header.stamp = ros::Time::now(); 34 | cp.header.frame_id = "/base_link"; 35 | cp.resolution = resolution; 36 | 37 | for(std::vector >::iterator it = cap_data.begin(); it != cap_data.end(); ++it) 38 | { 39 | map_creator::capShape cp_sp; 40 | cp_sp.identifier = (*it)[0]; 41 | cp_sp.ri = (*it)[1]; 42 | cp_sp.angleSFE = (*it)[2]; 43 | cp_sp.pose.position.x = (*it)[3]; 44 | cp_sp.pose.position.y = (*it)[4]; 45 | cp_sp.pose.position.z = (*it)[5]; 46 | cp_sp.pose.orientation.x = (*it)[6]; 47 | cp_sp.pose.orientation.x = (*it)[7]; 48 | cp_sp.pose.orientation.x = (*it)[8]; 49 | cp_sp.pose.orientation.x = (*it)[9]; 50 | cp.capShapes.push_back(cp_sp); 51 | 52 | } 53 | while (ros::ok()) 54 | { 55 | workspace_pub.publish(cp); 56 | ros::spinOnce(); 57 | sleep(1); 58 | ++count; 59 | } 60 | } 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /workspace_visualization/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 workspace_visualization 26 | { 27 | class ReachMapVisual; 28 | class ReachMapDisplay : public rviz::MessageFilterDisplay< map_creator::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 map_creator::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 workspace_visualization 88 | #endif // REACHABILITY_MAP_DISPLAY_H 89 | -------------------------------------------------------------------------------- /workspace_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | workspace_visualization 4 | 0.0.0 5 | The workspace_visualization package 6 | 7 | 8 | 9 | 10 | Abhijit Makhal 11 | 12 | 13 | 14 | 15 | 16 | Apache2.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Abhijit Makhal 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | rviz 45 | qtbase5-dev 46 | map_creator 47 | 48 | 49 | rviz 50 | libqt5-core 51 | libqt5-gui 52 | libqt5-widgets 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /map_creator/src/load_reachability_map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "map_creator/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 map_creator"); 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< map_creator::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 | // Creating messages 36 | map_creator::WorkSpace ws; 37 | ws.header.stamp = ros::Time::now(); 38 | ws.header.frame_id = "/base_link"; 39 | ws.resolution = res; 40 | 41 | for (MapVecDoublePtr::iterator it = sphere_col.begin(); it != sphere_col.end(); ++it) 42 | { 43 | map_creator::WsSphere wss; 44 | wss.point.x = (*it->first)[0]; 45 | wss.point.y = (*it->first)[1]; 46 | wss.point.z = (*it->first)[2]; 47 | wss.ri = it->second; 48 | 49 | for (MultiMapPtr::iterator it1 = pose_col_filter.lower_bound(it->first); it1 != pose_col_filter.upper_bound(it->first); ++it1) 50 | { 51 | geometry_msgs::Pose pp; 52 | pp.position.x = it1->second->at(0); 53 | pp.position.y = it1->second->at(1); 54 | pp.position.z = it1->second->at(2); 55 | pp.orientation.x = it1->second->at(3); 56 | pp.orientation.y = it1->second->at(4); 57 | pp.orientation.z = it1->second->at(5); 58 | pp.orientation.w = it1->second->at(6); 59 | wss.poses.push_back(pp); 60 | } 61 | ws.WsSpheres.push_back(wss); 62 | } 63 | 64 | while (ros::ok()) 65 | { 66 | workspace_pub.publish(ws); 67 | 68 | ros::spinOnce(); 69 | sleep(5); 70 | ++count; 71 | } 72 | } 73 | return 0; 74 | } 75 | -------------------------------------------------------------------------------- /base_placement_plugin/README.md: -------------------------------------------------------------------------------- 1 | ## Base Placement plugin 2 | ==== 3 | Please refer to [ros wiki] (http://wiki.ros.org/reuleaux) for more detailed instruction 4 | 5 | #1. RViz and RQT User Interface: 6 | 7 | There are two types of interactive markers: 8 | - The red arrow acts as a pointer which the user can move around the RViz enviroment. Fruthermore by clicking on the arrow another magenta arrow is added to the RViz enviroment. This arrow acts as task poses for base placement planner. 9 | - The magenta arrow is the task poses for the base placement planner. The orientation of the arrow can be changed by holding the CTRL key and moving it with the mouse. 10 | - Each arrow has a menu where the user can either delete the selected arrow or it can change its position and orientation by using the 6DOF marker control. 11 | - The RQT UI communicates simultaniously with the RViz enviroment and the User can change the state of a marker either through RViz or the RQT UI 12 | - TreeView displays all the added waypoints. The user can manipulate them directly in the TreeView and see their position and orientation of each waypoint. 13 | - The user can add new point or delete it through the RQT UI. 14 | - New tool component has been added for adding Arrows by using a mouse click 15 | 16 | #2. How to run the code 17 | Just run rosrun rviz rviz 18 | 19 | - From the panel window select base placement planner 20 | - Add a reachability map display and set the topic to /reachability_map 21 | - Add an Interactive Marker display and set the topic to /base_placement_plugin/update 22 | - Add a markerArray display. Set the topic to /visualization_marker_array 23 | 24 | There is a preconfigured rviz config file in the rviz folder 25 | 26 | After deciding the task poses load an inverse reachability map previously created. If you have not done it yet, run 27 | 28 | rosrun map_creator create_inverse_reachability_map 29 | 30 | rosrun map_creator create_inverse_reachability_map /home/abhi/Desktop/motomap_mh5_r0.08_reachability.h5 31 | 32 | it will save an invese reachability map in the map_creator/Inv_map folder 33 | 34 | After loading the inverse reachability map set the desired parameters. There are two parameters. Number of desired base locations and number of high scoring spheres from where the poses will be collected. 35 | 36 | Set the desired output visualization method.(The robot model visualization is still in development. Coming Soon) 37 | ``` 38 | When everything is set up, press the Find Base button. It will show the base locations. 39 | 40 | If you want to see the union map, press the show union map button. 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /map_creator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | map_creator 4 | 0.0.0 5 | The map_creator package 6 | 7 | 8 | 9 | 10 | Abhijit Makhal 11 | 12 | 13 | 14 | 15 | Apache 2.0 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | Abhijit Makhal 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | roscpp 43 | message_generation 44 | message_generation 45 | octomap 46 | rviz_visual_tools 47 | tf2 48 | hdf5 49 | pcl_ros 50 | 51 | roscpp 52 | visualization_msgs 53 | message_runtime 54 | octomap 55 | tf2 56 | hdf5 57 | pcl_ros 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/create_marker.h: -------------------------------------------------------------------------------- 1 | #ifndef CREATE_MARKER_H 2 | #define CREATE_MARKER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | typedef std::multimap,geometry_msgs::Pose> BasePoseJoint; 22 | 23 | class CreateMarker{ 24 | public: 25 | CreateMarker(std::string group_name); 26 | 27 | 28 | bool makeArmMarker(BasePoseJoint baseJoints, std::vector& iMarkers, bool show_unreachable_models); //visualizing robot model with joint solutions 29 | bool makeRobotMarker(BasePoseJoint baseJoints, std::vector& iMarkers, bool show_unreachable_models); //visualizing robot model with joint solutions 30 | bool checkEndEffector(); 31 | 32 | visualization_msgs::MarkerArray getDefaultMarkers(); 33 | 34 | 35 | 36 | 37 | private: 38 | 39 | void discardUnreachableModels(BasePoseJoint& baseJoints); 40 | void makeIntMarkers(BasePoseJoint& basePJoints, bool arm_only, std::vector &iMarkers); 41 | void updateRobotState(const std::vector& joint_soln, moveit::core::RobotStatePtr robot_state); 42 | void getFullLinkNames(std::vector& full_link_names, bool arm_only); 43 | void getArmLinks(std::vector& arm_links); 44 | void getEELinks(std::vector& ee_links); 45 | void makeIntMarkerControl(const geometry_msgs::Pose& base_pose, const std::vector& joint_soln,bool arm_only, bool is_reachable, visualization_msgs::InteractiveMarkerControl& robotModelControl); 46 | void createInteractiveMarker(const geometry_msgs::Pose& base_pose, const std::vector& joint_soln, 47 | const int& num, bool arm_only, bool is_reachable,visualization_msgs::InteractiveMarker& iMarker); 48 | 49 | void updateMarkers(const geometry_msgs::Pose& base_pose, bool is_reachable, Eigen::Affine3d tf_first_link_to_root, visualization_msgs::MarkerArray& markers); 50 | 51 | 52 | 53 | std::string group_name_; 54 | ros::AsyncSpinner spinner; 55 | boost::scoped_ptr group_; 56 | std::string parent_link; 57 | moveit::core::RobotModelConstPtr robot_model_; //Robot model const pointer 58 | }; 59 | 60 | #endif // CREATE_MARKER_H 61 | -------------------------------------------------------------------------------- /base_placement_plugin/src/point_tree_item.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | PointTreeItem::PointTreeItem(const QVector< QVariant > &data, PointTreeItem *parent) 6 | { 7 | parentItem = parent; 8 | itemData = data; 9 | } 10 | 11 | PointTreeItem::~PointTreeItem() 12 | { 13 | qDeleteAll(childItems); 14 | } 15 | 16 | PointTreeItem *PointTreeItem::child(int number) 17 | { 18 | return childItems.value(number); 19 | } 20 | 21 | int PointTreeItem::childCount() const 22 | { 23 | return childItems.count(); 24 | } 25 | 26 | int PointTreeItem::childNumber() const 27 | { 28 | if (parentItem) 29 | return parentItem->childItems.indexOf(const_cast< PointTreeItem * >(this)); 30 | 31 | return 0; 32 | } 33 | 34 | int PointTreeItem::columnCount() const 35 | { 36 | return itemData.count(); 37 | } 38 | 39 | QVariant PointTreeItem::data(int column) const 40 | { 41 | return itemData.value(column); 42 | } 43 | 44 | bool PointTreeItem::insertChildren(int position, int count, int columns) 45 | { 46 | if (position < 0 || position > childItems.size()) 47 | return false; 48 | 49 | for (int row = 0; row < count; ++row) 50 | { 51 | QVector< QVariant > data(columns); 52 | PointTreeItem *item = new PointTreeItem(data, this); 53 | childItems.insert(position, item); 54 | } 55 | 56 | return true; 57 | } 58 | 59 | bool PointTreeItem::insertColumns(int position, int columns) 60 | { 61 | if (position < 0 || position > itemData.size()) 62 | return false; 63 | 64 | for (int column = 0; column < columns; ++column) 65 | itemData.insert(position, QVariant()); 66 | 67 | PointTreeItem *child; 68 | for (int i = 0; i < childItems.count(); i++) 69 | { 70 | child = childItems.at(i); 71 | child->insertColumns(position, columns); 72 | } 73 | 74 | return true; 75 | } 76 | 77 | PointTreeItem *PointTreeItem::parent() 78 | { 79 | return parentItem; 80 | } 81 | 82 | bool PointTreeItem::removeChildren(int position, int count) 83 | { 84 | if (position < 0 || position + count > childItems.size()) 85 | return false; 86 | 87 | for (int row = 0; row < count; ++row) 88 | delete childItems.takeAt(position); 89 | 90 | return true; 91 | } 92 | 93 | bool PointTreeItem::removeColumns(int position, int columns) 94 | { 95 | if (position < 0 || position + columns > itemData.size()) 96 | return false; 97 | 98 | for (int column = 0; column < columns; ++column) 99 | itemData.remove(position); 100 | 101 | PointTreeItem *child; 102 | for (int i = 0; i < childItems.count(); i++) 103 | { 104 | child = childItems.at(i); 105 | child->removeColumns(position, columns); 106 | } 107 | 108 | return true; 109 | } 110 | 111 | bool PointTreeItem::setData(int column, const QVariant &value) 112 | { 113 | if (column < 0 || column >= itemData.size()) 114 | return false; 115 | 116 | itemData[column] = value; 117 | return true; 118 | } 119 | -------------------------------------------------------------------------------- /base_placement_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(base_placement_plugin) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS 7 | interactive_markers 8 | pluginlib 9 | roscpp 10 | rqt_gui 11 | rqt_gui_cpp 12 | rviz 13 | visualization_msgs 14 | tf 15 | tf_conversions 16 | map_creator 17 | pcl_ros 18 | geometry_msgs 19 | moveit_ros_planning_interface 20 | moveit_core 21 | ) 22 | 23 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 24 | 25 | include(${QT_USE_FILE}) 26 | 27 | 28 | add_definitions(-DQT_NO_KEYWORDS) 29 | 30 | 31 | add_library(create_marker src/create_marker.cpp) 32 | target_link_libraries(create_marker ${catkin_LIBRARIES} ) 33 | 34 | set(base_placement_plugin_SRCS 35 | src/add_way_point.cpp 36 | src/point_tree_item.cpp 37 | src/point_tree_model.cpp 38 | src/widgets/base_placement_widget.cpp 39 | src/place_base.cpp 40 | src/add_robot_base.cpp 41 | 42 | 43 | ${MOC_FILES} 44 | ) 45 | 46 | set(base_placement_plugin_HDRS 47 | include/base_placement_plugin/add_way_point.h 48 | include/base_placement_plugin/point_tree_item.h 49 | include/base_placement_plugin/point_tree_model.h 50 | include/base_placement_plugin/widgets/base_placement_widget.h 51 | include/base_placement_plugin/place_base.h 52 | include/base_placement_plugin/add_robot_base.h 53 | 54 | 55 | ) 56 | 57 | set(base_placement_plugin_UIS 58 | src/widgets/base_placement_widget.ui 59 | ) 60 | 61 | set(base_placement_plugin_INCLUDE_DIRECTORIES 62 | include 63 | ${CMAKE_CURRENT_BINARY_DIR} 64 | ) 65 | 66 | catkin_package( 67 | INCLUDE_DIRS ${base_placement_plugin_INCLUDE_DIRECTORIES} 68 | LIBRARIES ${PROJECT_NAME} 69 | CATKIN_DEPENDS 70 | interactive_markers 71 | pluginlib 72 | roscpp 73 | rqt_gui 74 | rviz 75 | rqt_gui_cpp 76 | visualization_msgs 77 | map_creator 78 | ) 79 | 80 | qt4_wrap_cpp(base_placement_plugin_MOCS ${base_placement_plugin_HDRS} ) 81 | qt4_wrap_ui(base_placement_plugin_UIS_H ${base_placement_plugin_UIS} ) 82 | 83 | include_directories(${base_placement_plugin_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS}) 84 | add_library(${PROJECT_NAME} ${base_placement_plugin_SRCS} ${base_placement_plugin_MOCS} ${base_placement_plugin_UIS_H}) # ${base_placement_plugin_UIS_H} 85 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY} yaml-cpp -lhdf5 -lhdf5_cpp create_marker) 86 | ## target_link_libraries(${PROJECT_NAME} yaml-cpp) 87 | 88 | 89 | 90 | find_package(class_loader) 91 | class_loader_hide_library_symbols(${PROJECT_NAME}) 92 | 93 | install(TARGETS ${PROJECT_NAME} 94 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 97 | ) 98 | 99 | install(PROGRAMS scripts/base_placement_plugin 100 | DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 101 | ) 102 | 103 | install(FILES plugin.xml 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 105 | ) 106 | 107 | -------------------------------------------------------------------------------- /map_creator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(map_creator) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | visualization_msgs 7 | roscpp 8 | message_generation 9 | geometry_msgs 10 | std_msgs 11 | tf2 12 | rviz_visual_tools 13 | pcl_ros 14 | moveit_ros_planning_interface 15 | moveit_core 16 | interactive_markers 17 | ) 18 | 19 | find_package(octomap REQUIRED) 20 | 21 | add_message_files( 22 | FILES 23 | capShape.msg 24 | capability.msg 25 | WsSphere.msg 26 | WorkSpace.msg 27 | 28 | ) 29 | 30 | generate_messages( 31 | DEPENDENCIES 32 | geometry_msgs std_msgs 33 | ) 34 | 35 | catkin_package( 36 | 37 | INCLUDE_DIRS 38 | include 39 | LIBRARIES 40 | sphere_discretization 41 | kinematics 42 | hdf5_dataset 43 | roscpp 44 | CATKIN_DEPENDS message_runtime 45 | ) 46 | 47 | include_directories(include ${catkin_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${HDF5_INCLUDE_DIR} ${PCL_INCLUDE_DIR}) 48 | 49 | add_library(sphere_discretization src/sphere_discretization.cpp) 50 | add_library(kinematics src/kinematics.cpp ) 51 | add_library(hdf5_dataset src/hdf5_dataset.cpp) 52 | 53 | target_link_libraries(sphere_discretization ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} ${PCL_LIBRARY_DIRS}) 54 | target_link_libraries(kinematics ${catkin_LIBRARIES} -llapack) 55 | target_link_libraries(hdf5_dataset ${catkin_LIBRARIES} -lhdf5 -lhdf5_cpp) 56 | 57 | install(TARGETS sphere_discretization kinematics hdf5_dataset 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | 61 | ) 62 | 63 | install(DIRECTORY include/${PROJECT_NAME}/ 64 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 65 | ) 66 | 67 | add_executable(create_reachability_map src/create_reachability_map.cpp) 68 | add_dependencies(create_reachability_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 69 | target_link_libraries(create_reachability_map sphere_discretization kinematics hdf5_dataset ${catkin_LIBRARIES}) 70 | 71 | add_executable(create_inverse_reachability_map src/create_inverse_reachability_map.cpp) 72 | add_dependencies(create_inverse_reachability_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 73 | target_link_libraries(create_inverse_reachability_map sphere_discretization kinematics hdf5_dataset ${catkin_LIBRARIES}) 74 | 75 | add_executable(create_capability_map src/create_capability_map.cpp) 76 | add_dependencies(create_capability_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 77 | target_link_libraries(create_capability_map sphere_discretization kinematics hdf5_dataset ${catkin_LIBRARIES}) 78 | 79 | add_executable(load_reachability_map src/load_reachability_map.cpp) 80 | add_dependencies(load_reachability_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 81 | target_link_libraries(load_reachability_map hdf5_dataset ${catkin_LIBRARIES}) 82 | 83 | add_executable(load_capability_map src/load_capability_map.cpp) 84 | add_dependencies(load_capability_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 85 | target_link_libraries(load_capability_map hdf5_dataset ${catkin_LIBRARIES}) 86 | 87 | 88 | if(CATKIN_ENABLE_TESTING) 89 | catkin_add_gtest(utest test/utest.cpp) 90 | target_link_libraries(utest sphere_discretization ${catkin_LIBRARIES}) 91 | endif() 92 | -------------------------------------------------------------------------------- /base_placement_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | base_placement_plugin 4 | 0.0.0 5 | The base_placement_plugin package 6 | 7 | 8 | 9 | 10 | Abhijit Makhal 11 | 12 | 13 | 14 | 15 | 16 | Apache 2.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Abhijit Makhal 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | interactive_markers 44 | pluginlib 45 | roscpp 46 | rqt_gui 47 | rqt_gui_cpp 48 | rviz 49 | visualization_msgs 50 | pcl_ros 51 | 52 | tf 53 | tf_conversions 54 | yaml-cpp 55 | map_creator 56 | 57 | 58 | 59 | interactive_markers 60 | pluginlib 61 | roscpp 62 | rqt_gui 63 | rqt_gui_cpp 64 | rviz 65 | visualization_msgs 66 | 67 | tf 68 | tf_conversions 69 | yaml-cpp 70 | map_creator 71 | pcl_ros 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /map_creator/include/map_creator/hdf5_dataset.h: -------------------------------------------------------------------------------- 1 | #ifndef HDF5_DATASET_H 2 | #define HDF5_DATASET_H 3 | #include "H5Cpp.h" 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 | bool open_cap(); //Opening the file, groups and database for capability map 31 | void close_cap();//Closing resources for cap map 32 | void close(); //Closing all the resources 33 | 34 | bool saveCapMapsToDataset(VectorOfVectors &capability_data, float &resolution); //Saves vector of capability data to multimap 35 | bool saveReachMapsToDataset( MultiMapPtr& poses, MapVecDoublePtr& spheres, float resolution); //Saves Mutimap and Map to database and closes 36 | 37 | bool loadCapMapFromDataset(VectorOfVectors &capability_data, float &resolution);//loads capability map and resolution data to 38 | bool loadMapsFromDataset(MultiMapPtr& poses, MapVecDoublePtr& spheres); //Creates exact same Poses MultiMap that was stored with address variation 39 | bool loadMapsFromDataset(MultiMapPtr& poses, MapVecDoublePtr& spheres, float &resolution); //with resolution 40 | bool loadMapsFromDataset(MultiMap& poses, MapVecDouble& spheres); //Loads the pose and sphere 41 | bool loadMapsFromDataset(MultiMap& Poses, MapVecDouble& Spheres, float &resolution); //with resolution 42 | bool h5ToResolution(float &resolution);//Accesses the resolution of the map 43 | 44 | private: 45 | 46 | bool h5ToVectorCap(VectorOfVectors &capability_data); //Accesses the capability data 47 | bool h5ToMultiMapPosesAndSpheres(MultiMapPtr& pose_col, MapVecDoublePtr& sphere_col); //loads the whole data with same address structure as stored in .h5 48 | 49 | bool h5ToMultiMapPoses(MultiMap& pose_col, MapVecDouble& sphere_col); //accesses the poses and spheres data in the poses dataset 50 | bool h5ToMultiMapPoses(MultiMap& pose_col); //Accessess only the data from poses dataset regardless of address 51 | bool h5ToMultiMapSpheres(MapVecDouble& sphere_col); //Accessess only the data from spheres dataset regardless of address 52 | 53 | bool checkPath(std::string path); //Checking if path exists 54 | bool checkFileName(std::string filename); //Checking if filename is a .h5 or not 55 | void createPath(std::string path); //Creating the path 56 | 57 | 58 | 59 | std::string path_; 60 | std::string filename_; 61 | hid_t file_, group_poses_, group_spheres_, group_capability_; 62 | hid_t poses_dataset_, sphere_dataset_, capability_dataset_; 63 | hid_t attr_; 64 | float res_; 65 | 66 | }; 67 | 68 | } // namespace hdf5_dataset 69 | #endif // HDF5_DATASET_H 70 | -------------------------------------------------------------------------------- /workspace_visualization/src/capability_map_display.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "capability_map_visual.h" 12 | 13 | #include "capability_map_display.h" 14 | 15 | namespace workspace_visualization 16 | { 17 | CapMapDisplay::CapMapDisplay() 18 | { 19 | is_byReachability_ = 20 | new rviz::BoolProperty("Color by Reachability", true, "Color transform by Reachability Index", this); 21 | disect_property_ = 22 | new rviz::EnumProperty("Disect", "Full", "Disection of the workspace", this, SLOT(updateColorAndAlpha())); 23 | disect_property_->addOption("Full", Full); 24 | disect_property_->addOption("1st_Half", First_Half); 25 | disect_property_->addOption("2nd_Half", Second_Half); 26 | disect_property_->addOption("Middle_Slice", Middle_Slice); 27 | disect_property_->addOption("End_Slice", End_Slice); 28 | 29 | color_property_ = new rviz::ColorProperty("Color", QColor(255, 225, 102), "Color to draw the Sphere.", this, 30 | SLOT(updateColorAndAlpha())); 31 | 32 | alpha_property_ = new rviz::FloatProperty("Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", this, 33 | SLOT(updateColorAndAlpha())); 34 | 35 | radius_property_ = new rviz::FloatProperty("Size", 0.1, "Size of the sphere", this, SLOT(updateSize())); 36 | 37 | lower_bound_reachability_ = new rviz::IntProperty("Lowest Reachability Index", 0, "Lowest Reachability index.", this); 38 | 39 | upper_bound_reachability_ = 40 | new rviz::IntProperty("Highest Reachability Index", 100, "Highest Reachability index.", this); 41 | } 42 | 43 | void CapMapDisplay::onInitialize() 44 | { 45 | MFDClass::onInitialize(); 46 | } 47 | 48 | CapMapDisplay::~CapMapDisplay() 49 | { 50 | } 51 | 52 | void CapMapDisplay::reset() 53 | { 54 | MFDClass::reset(); 55 | visuals_.clear(); 56 | } 57 | 58 | void CapMapDisplay::updateColorAndAlpha() 59 | { 60 | float alpha = alpha_property_->getFloat(); 61 | Ogre::ColourValue color = color_property_->getOgreColor(); 62 | 63 | for (size_t i = 0; i < visuals_.size(); i++) 64 | { 65 | visuals_[i]->setColor(color.r, color.g, color.b, alpha); 66 | } 67 | } 68 | 69 | void CapMapDisplay::updateSize() 70 | { 71 | float length = radius_property_->getFloat(); 72 | for (size_t i = 0; i < visuals_.size(); i++) 73 | { 74 | visuals_[i]->setSize(length); 75 | } 76 | } 77 | 78 | void CapMapDisplay::processMessage(const map_creator::capability::ConstPtr& msg) 79 | { 80 | Ogre::Quaternion orientation; 81 | Ogre::Vector3 position; 82 | if (!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation)) 83 | { 84 | ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), 85 | qPrintable(fixed_frame_)); 86 | return; 87 | } 88 | boost::shared_ptr< CapMapVisual > visual; 89 | visual.reset(new CapMapVisual(context_->getSceneManager(), scene_node_, context_)); 90 | visual->setMessage(msg, lower_bound_reachability_->getInt(), upper_bound_reachability_->getInt(), 91 | disect_property_->getOptionInt()); 92 | visual->setFramePosition(position); 93 | visual->setFrameOrientation(orientation); 94 | float alpha = alpha_property_->getFloat(); 95 | Ogre::ColourValue color = color_property_->getOgreColor(); 96 | if (is_byReachability_->getBool() == false) 97 | { 98 | visual->setColor(color.r, color.g, color.b, alpha); 99 | } 100 | else 101 | { 102 | visual->setColorbyRI(alpha); 103 | } 104 | visuals_.push_back(visual); 105 | updateSize(); 106 | } 107 | 108 | } // end namespace workspace_visualization 109 | #include 110 | PLUGINLIB_EXPORT_CLASS(workspace_visualization::CapMapDisplay, rviz::Display) 111 | -------------------------------------------------------------------------------- /map_creator/test/utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2016, Southwest Research Institute 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * * Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * * Neither the name of the Southwest Research Institute, nor the names 16 | * of its contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include 33 | #include 34 | 35 | using namespace sphere_discretization; 36 | //using sphere_discretization::findOptimalPosebyPCA; 37 | 38 | TEST(PCATest, TestCase1) 39 | { 40 | sphere_discretization::SphereDiscretization sd; 41 | 42 | const double DEFAULT_ALLOWED_VARATION = 0.01; 43 | std::vector< geometry_msgs::Pose > poses; 44 | geometry_msgs::Pose pose, pose1, pose2, final; 45 | 46 | pose1.position.x = 0; 47 | pose1.position.y = 0; 48 | pose1.position.z = 0; 49 | 50 | // normal unit quaternion (aligned with x-axis) 51 | pose1.orientation.x = 0; 52 | pose1.orientation.y = 0; 53 | pose1.orientation.z = 0; 54 | pose1.orientation.w = 1; 55 | 56 | poses.push_back(pose1); 57 | pose = pose1; 58 | // 90 degree rotation around y-axis (aligned with z-axis) 59 | pose1.orientation.x = 0; 60 | pose1.orientation.y = 0.707; 61 | pose1.orientation.z = 0; 62 | pose1.orientation.w = 0.707; 63 | 64 | poses.push_back(pose1); 65 | 66 | // final pose 67 | pose2 = pose1; 68 | pose2.orientation.x = 0.0; 69 | pose2.orientation.y = 0.383; 70 | pose2.orientation.z = 0.0; 71 | pose2.orientation.w = 0.924; 72 | 73 | sd.findOptimalPosebyPCA(poses, final); 74 | EXPECT_NEAR(final.orientation.x, pose2.orientation.x, DEFAULT_ALLOWED_VARATION); 75 | EXPECT_NEAR(final.orientation.y, pose2.orientation.y, DEFAULT_ALLOWED_VARATION); 76 | EXPECT_NEAR(final.orientation.z, pose2.orientation.z, DEFAULT_ALLOWED_VARATION); 77 | EXPECT_NEAR(final.orientation.w, pose2.orientation.w, DEFAULT_ALLOWED_VARATION); 78 | 79 | // 90 degree rotation around y-axis (aligned with z-axis) 80 | pose1.orientation.x = 0; 81 | pose1.orientation.y = -0.707; 82 | pose1.orientation.z = 0; 83 | pose1.orientation.w = 0.707; 84 | 85 | poses.push_back(pose1); 86 | 87 | sd.findOptimalPosebyPCA(poses, final); 88 | EXPECT_NEAR(final.orientation.x, pose.orientation.x, DEFAULT_ALLOWED_VARATION); 89 | EXPECT_NEAR(final.orientation.y, pose.orientation.y, DEFAULT_ALLOWED_VARATION); 90 | EXPECT_NEAR(final.orientation.z, pose.orientation.z, DEFAULT_ALLOWED_VARATION); 91 | EXPECT_NEAR(final.orientation.w, pose.orientation.w, DEFAULT_ALLOWED_VARATION); 92 | 93 | } 94 | 95 | 96 | // Run all the tests that were declared with TEST() 97 | int main(int argc, char **argv) 98 | { 99 | ros::init(argc, argv, "test"); // some tests need ROS framework 100 | testing::InitGoogleTest(&argc, argv); 101 | return RUN_ALL_TESTS(); 102 | } 103 | 104 | 105 | -------------------------------------------------------------------------------- /map_creator/README.md: -------------------------------------------------------------------------------- 1 | ## Map Creator 2 | ==== 3 | ***All the Inverse Kinematics solutions for Reuleaux is generated by ikfast. The generated solutions from ikfast for the desired robot should be linked with the header files of Reuleaux. Please refer to the [http://wiki.ros.org/reuleaux] (http://wiki.ros.org/reuleaux) page for the process of generating ikfast solution for your robot and linking the ikfast solution to Reuleaux. The default robot provided in the package is motorman_mh5. You can easily import your robot by the instructions.*** 4 | 5 | Reuleaux map_creator pacakge creates three types of maps. (You don’t have to create all the maps, it is per your needs): 6 | ###1. Reachability map 7 | The Reachability map describes the reachability of a given robot model by discretizing its environment, creating poses in the environment and calculating valid IK solutions for the poses. The poses which are reachable by robot are associated with discretized spheres. The reachability of each sphere in the environment are parameterized, by a Reachability index. The output is saved as an hdf5 file (link for hdf5 file) which has details about all the reachable poses and discretized spheres. There are mainly two user arguments: 8 | 9 | a) Resolution parameter: The first step of the map generation process is discretization of the environment by voxelization. The resolution determines how much small the boxes would be. The smaller voxels, the higher the number of poses per spheres. (**Believe me, it grows exponentially. Please do not try to go too low with the resolution. The safe threshold is 0.05. Less than that, can take hours, or your system may stop responding**). If the user does not provide any resolution, the default resolution is 0.08 10 | 11 | b) Map filename: The second argument decides the output filename. If the user does not provide an ouput filename, it will automatically decide an ugly map name with the robot name and provided resolution. 12 | To create a reachability map, run: 13 | 14 | rosrun map_creator create_reachability_map 15 | 16 | with arguments: 17 | 18 | rosrun map_creator create_reachability_map funny_robot.h5 0.05 19 | 20 | When the process finishes, the output reachability map will be stored in map_creator/maps folder. If you do not have the existing maps folder, do not worry. It will create a map folder in the map_creator package and store the output there. 21 | 22 | ###2. Capability Map (optional) 23 | Capability map is an extension of reachability map (I guess you have already done that, otherwise please create a reachability map first), where the outer spheres of the reachability map, is set as cones. So the reachability limit of the robot is well visualized. All the outer spheres are decided for a principal axes and iterates over different values for opening angles for cones. The suitable opening angle that correctly accumulates all the poses on that sphere, is picked up 24 | (** Until we found out some very useful algorithm for capability map, the process may take several hours based on resolution) 25 | The process is same as creating reachability map: 26 | 27 | rosrun map_creator create_capability_map 28 | 29 | The ouput map file will also be stored in map_creator/maps folder. 30 | 31 | 32 | ###3. Inverse Reachability Map 33 | The purpose of Inverse Reachability map is to find suitable base positions for a robot with given task poses. To know how to find suitable bases, please refer to (base_placement plugin page) 34 | The inverse reachability map is a general inverse transformation of all the reachable poses of the reachability map of the robot. The user have to provide the reachability map as an argument. The desired name of the ouput file can also be provided. If no output file name is provided, the system will automatically generate a map file with the robot name and resolution provided in the reachability map. To create an inverse reachability map: 35 | 36 | rosrun map_creator create_inverse_reachability_map motoman_mh5_r0.08_reachability.h5 37 | 38 | (provided reachability map is of motoman_mh5 robot and resolution is 0.08). The outout inverse reachability map will be stored in map_creator/Inv_map folder unless specified otherwise. 39 | 40 | (Congratulations. You have now also created the inverse reachability map which is the key element for finding bases. Now it is the time to see the result of all the hard work. Let’s move to the [visualization] (https://github.com/ros-industrial-consortium/reuleaux/tree/master/workspace_visualization) page) 41 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/add_robot_base.h: -------------------------------------------------------------------------------- 1 | #ifndef ADD_ROBOT_BASE_H 2 | #define ADD_ROBOT_BASE_H 3 | #ifndef Q_MOC_RUN 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #endif 43 | 44 | 45 | namespace rviz 46 | { 47 | class VectorProperty; 48 | class VisualizationManager; 49 | class ViewportMouseEvent; 50 | class StringProperty; 51 | class BoolProperty; 52 | } 53 | 54 | 55 | class AddRobotBase : public QObject 56 | { 57 | Q_OBJECT; 58 | public: 59 | AddRobotBase(QWidget* parent, std::string group_name); 60 | virtual ~AddRobotBase(); 61 | void init(); 62 | 63 | virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 64 | 65 | void changeMarkerControlAndPose(std::string marker_name, bool set_control); 66 | visualization_msgs::InteractiveMarkerControl& makeArrowControlDefault(visualization_msgs::InteractiveMarker& msg); 67 | visualization_msgs::InteractiveMarkerControl& makeArrowControlDetails(visualization_msgs::InteractiveMarker& msg); 68 | visualization_msgs::Marker makeWayPoint(visualization_msgs::InteractiveMarker& msg); 69 | void pointDeleted(std::string marker_name); 70 | void makeArrow(const tf::Transform& point_pos, int count_arrow); 71 | void makeInteractiveMarker(); 72 | visualization_msgs::InteractiveMarkerControl& makeInteractiveMarkerControl(visualization_msgs::InteractiveMarker& msg); 73 | visualization_msgs::Marker makeInterArrow(visualization_msgs::InteractiveMarker& msg); 74 | visualization_msgs::MarkerArray makeRobotMarker(visualization_msgs::InteractiveMarker& msg, bool waypoint); 75 | 76 | 77 | void pointPoseUpdated(const tf::Transform& point_pos, const char* marker_name); 78 | void getWaypoints(std::vector& waypoints); 79 | 80 | 81 | 82 | public Q_SLOTS: 83 | void parseWayPoints(); 84 | void clearAllPointsRviz(); 85 | void getRobotModelFrame_slot(const tf::Transform end_effector); 86 | 87 | protected: 88 | QWidget* widget_; 89 | 90 | Q_SIGNALS: 91 | void baseWayPoints_signal(std::vector base_poses); 92 | 93 | private: 94 | //! Define a server for the Interactive Markers. 95 | boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server; 96 | interactive_markers::MenuHandler menu_handler; 97 | 98 | //! for arrows...........need to be modified 99 | std_msgs::ColorRGBA WAY_POINT_COLOR; 100 | std_msgs::ColorRGBA ROBOT_WAY_POINT_COLOR; 101 | std_msgs::ColorRGBA ARROW_INTER_COLOR; 102 | std_msgs::ColorRGBA ROBOT_INTER_COLOR; 103 | geometry_msgs::Vector3 WAY_POINT_SCALE_CONTROL; 104 | geometry_msgs::Vector3 ARROW_INTER_SCALE_CONTROL; 105 | 106 | float INTERACTIVE_MARKER_SCALE; 107 | float ARROW_INTERACTIVE_SCALE; 108 | 109 | //! Vector for storing all the User Entered Way-Points. 110 | std::vector< tf::Transform > waypoints_pos; 111 | //! The position of the User handlable Interactive Marker. 112 | tf::Transform box_pos; 113 | int count; 114 | std::string target_frame_; 115 | 116 | visualization_msgs::MarkerArray robot_markers_; 117 | CreateMarker* mark_; 118 | std::string group_name_; 119 | 120 | }; 121 | 122 | 123 | 124 | 125 | 126 | 127 | #endif // ADD_ROBOT_BASE_H 128 | -------------------------------------------------------------------------------- /workspace_visualization/src/capability_map_visual.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "capability_map_visual.h" 12 | 13 | namespace workspace_visualization 14 | { 15 | CapMapVisual::CapMapVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, 16 | rviz::DisplayContext* display_context) 17 | { 18 | scene_manager_ = scene_manager; 19 | frame_node_ = parent_node->createChildSceneNode(); 20 | } 21 | 22 | CapMapVisual::~CapMapVisual() 23 | { 24 | scene_manager_->destroySceneNode(frame_node_); 25 | } 26 | 27 | void CapMapVisual::setMessage(const map_creator::capability::ConstPtr& msg, int low_ri, int high_ri, int disect_choice) 28 | { 29 | int low_SphereSize, up_SphereSize; 30 | switch (disect_choice) 31 | { 32 | case 0: 33 | { 34 | low_SphereSize = 0; 35 | up_SphereSize = msg->capShapes.size(); 36 | break; 37 | } 38 | case 1: 39 | { 40 | low_SphereSize = 0; 41 | up_SphereSize = msg->capShapes.size() / 2; 42 | break; 43 | } 44 | case 2: 45 | { 46 | low_SphereSize = msg->capShapes.size() / 2; 47 | up_SphereSize = msg->capShapes.size(); 48 | break; 49 | } 50 | case 3: 51 | { 52 | low_SphereSize = msg->capShapes.size() / 2.2; 53 | up_SphereSize = msg->capShapes.size() / 1.8; 54 | break; 55 | } 56 | case 4: 57 | { 58 | low_SphereSize = 0; 59 | up_SphereSize = msg->capShapes.size() / 1.1; 60 | break; 61 | } 62 | } 63 | 64 | boost::shared_ptr< rviz::Shape > sphere_center; 65 | int colorRI; 66 | for (size_t i = low_SphereSize; i < up_SphereSize; ++i) 67 | { 68 | if (low_ri < int(msg->capShapes[i].ri) && int(msg->capShapes[i].ri) <= high_ri) 69 | { 70 | if (msg->capShapes[i].identifier == 2.0) 71 | { 72 | sphere_center.reset(new rviz::Shape(rviz::Shape::Sphere, scene_manager_, frame_node_)); 73 | } 74 | 75 | else 76 | { 77 | sphere_center.reset(new rviz::Shape(rviz::Shape::Cone, scene_manager_, frame_node_)); 78 | } 79 | 80 | Ogre::Vector3 position_(msg->capShapes[i].pose.position.x, msg->capShapes[i].pose.position.y, 81 | msg->capShapes[i].pose.position.z); 82 | tf2::Quaternion quat(msg->capShapes[i].pose.orientation.x, msg->capShapes[i].pose.orientation.y, 83 | msg->capShapes[i].pose.orientation.z, msg->capShapes[i].pose.orientation.w); 84 | 85 | quat.normalize(); 86 | Ogre::Quaternion orientation_(quat.w(), quat.x(), quat.y(), quat.z()); 87 | 88 | if (position_.isNaN() || orientation_.isNaN()) 89 | { 90 | ROS_WARN("received invalid pose"); 91 | return; 92 | } 93 | sphere_center->setPosition(position_); 94 | sphere_center->setOrientation(orientation_); 95 | 96 | sphere_.push_back(sphere_center); 97 | colorRI = msg->capShapes[i].ri; 98 | colorRI_.push_back(colorRI); 99 | } 100 | } 101 | } 102 | 103 | void CapMapVisual::setFramePosition(const Ogre::Vector3& position) 104 | { 105 | frame_node_->setPosition(position); 106 | } 107 | 108 | void CapMapVisual::setFrameOrientation(const Ogre::Quaternion& orientation) 109 | { 110 | frame_node_->setOrientation(orientation); 111 | } 112 | 113 | void CapMapVisual::setColor(float r, float g, float b, float a) 114 | { 115 | for (int i = 0; i < sphere_.size(); ++i) 116 | { 117 | sphere_[i]->setColor(r, g, b, a); 118 | } 119 | } 120 | 121 | void CapMapVisual::setColorbyRI(float alpha) 122 | { 123 | for (int i = 0; i < sphere_.size(); ++i) 124 | { 125 | if (colorRI_[i] >= 90) 126 | { 127 | sphere_[i]->setColor(0, 0, 255, alpha); 128 | } 129 | else if (colorRI_[i] < 90 && colorRI_[i] >= 50) 130 | { 131 | sphere_[i]->setColor(0, 255, 255, alpha); 132 | } 133 | else if (colorRI_[i] < 50 && colorRI_[i] >= 30) 134 | { 135 | sphere_[i]->setColor(0, 255, 0, alpha); 136 | } 137 | else if (colorRI_[i] < 30 && colorRI_[i] >= 5) 138 | { 139 | sphere_[i]->setColor(255, 255, 0, alpha); 140 | } 141 | else 142 | { 143 | sphere_[i]->setColor(255, 0, 0, alpha); 144 | } 145 | } 146 | } 147 | 148 | void CapMapVisual::setSize(float l) 149 | { 150 | for (int i = 0; i < sphere_.size(); ++i) 151 | { 152 | sphere_[i]->setScale(Ogre::Vector3(l, l, l)); 153 | } 154 | } 155 | 156 | } // end namespace workspace_visualization 157 | -------------------------------------------------------------------------------- /map_creator/include/map_creator/sphere_discretization.h: -------------------------------------------------------------------------------- 1 | #ifndef SPHERE_DISCRETIZATION_H 2 | #define SPHERE_DISCRETIZATION_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "geometry_msgs/Pose.h" 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | //#include 21 | 22 | namespace sphere_discretization 23 | { 24 | class SphereDiscretization 25 | { 26 | public: 27 | SphereDiscretization(){} 28 | 29 | ~SphereDiscretization(){} 30 | 31 | //! Generating a sphere octree by insertRay 32 | octomap::OcTree* generateSphereTree(const octomap::point3d& origin, float radius, float resolution); 33 | 34 | //! Generating a sphere octree by creating poincloud and pushing to octree 35 | octomap::OcTree* generateSphereTree2(const octomap::point3d& origin, float radius, float resolution); 36 | 37 | //! Generating a box octree 38 | octomap::OcTree* generateBoxTree(const octomap::point3d& origin, float diameter, float resolution); 39 | 40 | //! Creating a sphere with points and return poincloud 41 | octomap::Pointcloud make_sphere_points(const octomap::point3d& origin, double r); 42 | 43 | //! Creating a sphere and and create poses on the outer sphere and return vector of poses 44 | void make_sphere_poses(const octomap::point3d &origin, double r, std::vector< geometry_msgs::Pose >& pose_Col); 45 | 46 | //! Creating random doubles 47 | double irand(int min, int max); 48 | 49 | //! Creating sphere with random points 50 | octomap::Pointcloud make_sphere_rand(octomap::point3d origin, double r, int sample); 51 | 52 | //! Creating sphere by Archimedes theorem 53 | octomap::Pointcloud make_sphere_Archimedes(octomap::point3d origin, double r, int sample); 54 | 55 | //! Creating sphere by Fibonacci grid 56 | octomap::Pointcloud make_sphere_fibonacci_grid(octomap::point3d origin, double r, int sample); 57 | 58 | //! Creates sphere spiral points and returns pointcloud 59 | octomap::Pointcloud make_sphere_spiral_points(const octomap::point3d &origin, double r, int sample); 60 | 61 | //! Creates long lat grid on a sphere 62 | octomap::Pointcloud make_long_lat_grid(const octomap::point3d &origin, double r, int sample, int lat_num, int lon_num); 63 | 64 | //! Returns non-negative numbers of R8 division 65 | double r8_modp(double x, double y); 66 | 67 | //! Convering vector[3] to point 68 | void convertPointToVector(const octomap::point3d point, std::vector< double >& data); 69 | 70 | //! Converting point to vector[3] 71 | void convertVectorToPoint(const std::vector< double > data, octomap::point3d& point); 72 | 73 | //! Converting geometry_msgs::Pose to vector[7] 74 | void convertPoseToVector(const geometry_msgs::Pose &pose, std::vector< double >& data); 75 | 76 | //! Converting vector[7] to geometry_msgs::Pose 77 | void convertVectorToPose(const std::vector< double >& data, geometry_msgs::Pose& pose); 78 | 79 | //! Finding optimal pose by averaging all and returning pose 80 | geometry_msgs::Pose findOptimalPose(const std::vector< geometry_msgs::Pose >& poses, const octomap::point3d& origin); 81 | 82 | //! Creates cone by PointCloud 83 | void createConeCloud(const geometry_msgs::Pose &pose, const double angle, const double scale, 84 | pcl::PointCloud< pcl::PointXYZ >::Ptr cloud); 85 | 86 | //! Creates point by removing orientation part of a pose 87 | void poseToPoint(const geometry_msgs::Pose& pose, octomap::point3d& point); 88 | 89 | //! Defines if a point belongs to a pointcloud. First crates a hull of the poincloud and computes Area, then adds the 90 | //point to the pointcloud and again computes new Area. If the New Area is bigger than the previous area, the point is 91 | //outside poincloud, otherwise inside 92 | bool isPointInCloud(pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, octomap::point3d point); 93 | 94 | //! Finds L2 norm distance between two points 95 | float distanceL2norm(const octomap::point3d& p1, const octomap::point3d& p2); 96 | 97 | //! Converts geometry_msgs::Pose to Eigen Vector 98 | void poseToEigenVector(const geometry_msgs::Pose& pose, Eigen::VectorXd& vec); 99 | 100 | //! Finds optimal pose of given poses by Principal Component Optimization 101 | void findOptimalPosebyPCA(const std::vector< geometry_msgs::Pose >& probBasePoses, geometry_msgs::Pose& final_base_pose); 102 | 103 | //! Finds if two quaternions are close 104 | bool areQuaternionClose(const tf2::Quaternion &q1, const tf2::Quaternion &q2); 105 | 106 | //! Computes the inverse signature of a quaternion 107 | tf2::Quaternion inverseSignQuaternion(const tf2::Quaternion& q); 108 | 109 | //! Finds optimal pose of given poses by average 110 | void findOptimalPosebyAverage(const std::vector< geometry_msgs::Pose > probBasePoses, 111 | geometry_msgs::Pose& final_base_pose); 112 | 113 | //! Given grasp poses and multimap structure of an inverse reachability map, transforms every pose of the ir map with 114 | //grasp poses, and calculates nearest neighbor search to associate poses with belonging spheres. 115 | void associatePose(std::multimap, std::vector > &baseTrnsCol, 116 | const std::vector< geometry_msgs::Pose >& grasp_poses, 117 | const std::multimap, std::vector > &PoseColFilter, const float resolution); 118 | 119 | //! Compare two vectors, of length 3, for multimap search 120 | struct vec_comp_ 121 | { 122 | bool operator()(const std::vector< float >& v1, const std::vector< float >& v2) const 123 | { 124 | // TODO: need to add tolerance as a function of the map resolution; resolution maybe needs to be a class variable 125 | // but this appears to work fine for now 126 | float tol = 0.001; 127 | return (fabs(v1[0] - v2[0]) < tol) && (fabs(v1[1] - v2[1]) < tol) && (fabs(v1[2] - v2[2]) < tol); 128 | } 129 | }; 130 | }; 131 | 132 | } // namespace sphere_discretization 133 | #endif // SPHERE_DISCRETIZATION_H 134 | -------------------------------------------------------------------------------- /base_placement_plugin/rviz/base_placement.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 280 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | - Class: base_placement_plugin/Base Placement Plug-in 31 | Name: Base Placement Plug-in 32 | TextEntry: test_field 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: workspace_visualization/ReachabilityMap 55 | Color by Reachability: true 56 | Disect: Full 57 | Enabled: true 58 | Highest Reachability Index: 100 59 | Lowest Reachability Index: 0 60 | Name: ReachabilityMap 61 | Poses Property: 62 | Alpha: 0.2 63 | Color: 204; 51; 204 64 | Length: 0.01 65 | Shape: Sphere 66 | Shape Property: 67 | Alpha: 1 68 | Color: 255; 225; 102 69 | Size: 0.05 70 | Show Poses: false 71 | Show Shape: true 72 | Topic: /reachability_map 73 | Unreliable: false 74 | Value: true 75 | - Class: rviz/MarkerArray 76 | Enabled: true 77 | Marker Topic: visualization_marker_array 78 | Name: MarkerArray 79 | Namespaces: 80 | {} 81 | Queue Size: 100 82 | Value: true 83 | - Class: rviz/InteractiveMarkers 84 | Enable Transparency: true 85 | Enabled: true 86 | Name: InteractiveMarkers 87 | Show Axes: false 88 | Show Descriptions: true 89 | Show Visual Aids: false 90 | Update Topic: /base_placement_plugin/update 91 | Value: true 92 | - Class: rviz/InteractiveMarkers 93 | Enable Transparency: true 94 | Enabled: true 95 | Name: InteractiveMarkers 96 | Show Axes: true 97 | Show Descriptions: false 98 | Show Visual Aids: false 99 | Update Topic: /robot_model/update 100 | Value: true 101 | - Class: rviz/InteractiveMarkers 102 | Enable Transparency: true 103 | Enabled: true 104 | Name: InteractiveMarkers 105 | Show Axes: false 106 | Show Descriptions: true 107 | Show Visual Aids: false 108 | Update Topic: /user_intutition/update 109 | Value: true 110 | Enabled: true 111 | Global Options: 112 | Background Color: 48; 48; 48 113 | Fixed Frame: base_link 114 | Frame Rate: 30 115 | Name: root 116 | Tools: 117 | - Class: rviz/Interact 118 | Hide Inactive Objects: true 119 | - Class: rviz/MoveCamera 120 | - Class: rviz/Select 121 | - Class: rviz/FocusCamera 122 | - Class: rviz/Measure 123 | - Class: rviz/SetInitialPose 124 | Topic: /initialpose 125 | - Class: rviz/SetGoal 126 | Topic: /move_base_simple/goal 127 | - Class: rviz/PublishPoint 128 | Single click: true 129 | Topic: /clicked_point 130 | Value: true 131 | Views: 132 | Current: 133 | Class: rviz/Orbit 134 | Distance: 3.8916 135 | Enable Stereo Rendering: 136 | Stereo Eye Separation: 0.06 137 | Stereo Focal Distance: 1 138 | Swap Stereo Eyes: false 139 | Value: false 140 | Focal Point: 141 | X: 0 142 | Y: 0 143 | Z: 0 144 | Name: Current View 145 | Near Clip Distance: 0.01 146 | Pitch: 0.440398 147 | Target Frame: 148 | Value: Orbit (rviz) 149 | Yaw: 6.1386 150 | Saved: ~ 151 | Window Geometry: 152 | Base Placement Plug-in: 153 | collapsed: false 154 | Displays: 155 | collapsed: false 156 | Height: 1026 157 | Hide Left Dock: false 158 | Hide Right Dock: true 159 | QMainWindow State: 000000ff00000000fd00000004000000000000023d0000036efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003600000155000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002c004200610073006500200050006c006100630065006d0065006e007400200050006c00750067002d0069006e010000019100000213000001f600ffffff000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006900000003efc0100000002fb0000000800540069006d00650100000000000006900000024600fffffffb0000000800540069006d006501000000000000045000000000000000000000044d0000036e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 160 | Selection: 161 | collapsed: false 162 | Time: 163 | collapsed: false 164 | Tool Properties: 165 | collapsed: false 166 | Views: 167 | collapsed: true 168 | Width: 1680 169 | X: 1920 170 | Y: 24 171 | -------------------------------------------------------------------------------- /workspace_visualization/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 workspace_visualization 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 map_creator::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 | visual.reset(new ReachMapVisual(context_->getSceneManager(), scene_node_, context_)); 137 | 138 | visual->setMessage(msg, do_display_arrow_->getBool(), do_display_sphere_->getBool(), 139 | lower_bound_reachability_->getInt(), upper_bound_reachability_->getInt(), 140 | shape_property_->getOptionInt(), disect_property_->getOptionInt()); 141 | 142 | visual->setFramePosition(position); 143 | visual->setFrameOrientation(orientation); 144 | 145 | float arrow_alpha = arrow_alpha_property_->getFloat(); 146 | Ogre::ColourValue arrow_color = arrow_color_property_->getOgreColor(); 147 | visual->setColorArrow(arrow_color.r, arrow_color.g, arrow_color.b, arrow_alpha); 148 | 149 | float sphere_alpha = sphere_alpha_property_->getFloat(); 150 | Ogre::ColourValue sphere_color = sphere_color_property_->getOgreColor(); 151 | if (is_byReachability_->getBool() == false) 152 | { 153 | visual->setColorSphere(sphere_color.r, sphere_color.g, sphere_color.b, sphere_alpha); 154 | } 155 | else 156 | { 157 | visual->setColorSpherebyRI(sphere_alpha); 158 | } 159 | 160 | visuals_.push_back(visual); 161 | //updateArrowSize(); 162 | updateSphereSize(); 163 | 164 | } 165 | 166 | } // end namespace workspace_visualization 167 | #include 168 | PLUGINLIB_EXPORT_CLASS(workspace_visualization::ReachMapDisplay, rviz::Display) 169 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/add_way_point.h: -------------------------------------------------------------------------------- 1 | #ifndef add_way_point_H_ 2 | #define add_way_point_H_ 3 | #ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #endif 44 | 45 | namespace rviz 46 | { 47 | class VectorProperty; 48 | class VisualizationManager; 49 | class ViewportMouseEvent; 50 | 51 | class StringProperty; 52 | class BoolProperty; 53 | } 54 | 55 | namespace base_placement_plugin 56 | { 57 | /*! 58 | * \brief Class for handling the User Interactions with the RViz enviroment. 59 | * \details The AddWayPoint Class handles all the Visualization in the RViz enviroment. 60 | This Class inherits from the rviz::Panel superclass. 61 | * \author Risto Kojcev 62 | */ 63 | class AddWayPoint : public rviz::Panel 64 | { 65 | Q_OBJECT 66 | public: 67 | //! A Constructor for the RViz Panel. 68 | AddWayPoint(QWidget* parent = 0); 69 | //! Virtual Destructor for the RViz Panel. 70 | virtual ~AddWayPoint(); 71 | //! RViz panel initialization 72 | virtual void onInitialize(); 73 | 74 | //! Fucntion for all the interactive marker interactions 75 | virtual void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 76 | 77 | //! Make a new Interactive Marker Way-Point 78 | virtual void makeArrow(const tf::Transform& point_pos, int count_arrow); 79 | //! User Interaction Arrow Marker 80 | virtual void makeInteractiveMarker(); 81 | 82 | private: 83 | //! Function for creating a way-point marker 84 | visualization_msgs::Marker makeWayPoint(visualization_msgs::InteractiveMarker& msg); 85 | //! Function to create the InteractionArrow Marker 86 | visualization_msgs::Marker makeInterArrow(visualization_msgs::InteractiveMarker& msg); 87 | //! Create controls for each different marker. Here we have control for the defaulot starting control ArrowMarkers 88 | visualization_msgs::InteractiveMarkerControl& makeArrowControlDefault(visualization_msgs::InteractiveMarker& msg); 89 | //! 6DOF control for the Ingteractive Markers 90 | visualization_msgs::InteractiveMarkerControl& makeArrowControlDetails(visualization_msgs::InteractiveMarker& msg); 91 | 92 | //! The box control can be used as a pointer to a certain 3D location and when clicked it will add a arrow to that 93 | // location. 94 | visualization_msgs::InteractiveMarkerControl& makeInteractiveMarkerControl(visualization_msgs::InteractiveMarker& msg_box); 95 | //! Function to handle the entries made from the Way-Points interactive markers Menu. 96 | virtual void changeMarkerControlAndPose(std::string marker_name, bool set_control); 97 | 98 | //! Define a server for the Interactive Markers. 99 | boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server; 100 | interactive_markers::MenuHandler menu_handler; 101 | 102 | //! Vector for storing all the User Entered Way-Points. 103 | std::vector< tf::Transform > waypoints_pos; 104 | //! The position of the User handlable Interactive Marker. 105 | tf::Transform box_pos; 106 | 107 | //! Variable for storing the count of the Way-Points. 108 | int count; 109 | //! Target Frame for the Transformation. 110 | std::string target_frame_; 111 | 112 | protected Q_SLOTS: 113 | //! rviz::Panel virtual functions for loading Panel Configuration. 114 | virtual void load(const rviz::Config& config); 115 | //! rviz::Panel virtual functions for saving Panel Configuration. 116 | virtual void save(rviz::Config config) const; 117 | public Q_SLOTS: 118 | //! Slot for handling the event of way-point deletion. 119 | virtual void pointDeleted(std::string marker_name); 120 | //! Slot for handling the add way-point event from the RQT UI. 121 | void addPointFromUI(const tf::Transform point_pos); 122 | //! Slot for handling when the user updates the position of the Interactive Markers. 123 | void pointPoseUpdated(const tf::Transform& point_pos, const char* marker_name); 124 | //! Slot for parsing the Way-Points before sending them to the MoveIt class. 125 | void parseWayPoints(); 126 | //! Save all the Way-Points to a yaml file. 127 | void saveWayPointsToFile(); 128 | //! Clear all the Way-Points 129 | void clearAllPointsRViz(); 130 | 131 | //! Get the name of the Transformation frame of the Robot. 132 | void getRobotModelFrame_slot(const tf::Transform end_effector); 133 | 134 | Q_SIGNALS: 135 | //! Signal for notifying that RViz is done with initialization. 136 | void initRviz(); 137 | //! Signal for notifying that a way-point was deleted in the RViz enviroment. 138 | void pointDeleteRviz(int marker_name_nr); 139 | //! Signal for notifying that a way-point has been added from the RViz enviroment. 140 | void addPointRViz(const tf::Transform& point_pos, const int count); 141 | //! Signal that the way-point position has been updated by the user from the RViz enviroment. 142 | void pointPoseUpdatedRViz(const tf::Transform& point_pos, const char* marker_name); 143 | //! Signal for sending all the Way-Points. 144 | void wayPoints_signal(std::vector< geometry_msgs::Pose > waypoints); 145 | //! Signal to check if the way-point is in valid IK solution. 146 | // void onUpdatePosCheckIkValidity(const geometry_msgs::Pose& waypoint,const int point_number); 147 | 148 | protected: 149 | //! The class that GUI RQT User Interactions. 150 | QWidget* widget_; 151 | //! The Object for the MoveIt components. 152 | QObject* place_base; 153 | 154 | private: 155 | //! Define constants for color, arrow size, etc. 156 | std_msgs::ColorRGBA WAY_POINT_COLOR; 157 | std_msgs::ColorRGBA ARROW_INTER_COLOR; 158 | 159 | geometry_msgs::Vector3 WAY_POINT_SCALE_CONTROL; 160 | geometry_msgs::Vector3 ARROW_INTER_SCALE_CONTROL; 161 | 162 | float INTERACTIVE_MARKER_SCALE; 163 | float ARROW_INTERACTIVE_SCALE; 164 | }; 165 | } // end of namespace base_placement_plugin 166 | 167 | #endif // add_way_point_H_ 168 | -------------------------------------------------------------------------------- /base_placement_plugin/src/point_tree_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | PointTreeModel::PointTreeModel(const QStringList &headers, const QString &data, QObject *parent) 7 | : QAbstractItemModel(parent) 8 | { 9 | QVector< QVariant > rootData; 10 | // QString header; 11 | 12 | for (int i = 0; i < headers.count(); i++) 13 | rootData << headers.at(i); 14 | 15 | rootItem = new PointTreeItem(rootData); 16 | setupModelData(data.split(QString("\n")), rootItem); 17 | } 18 | 19 | PointTreeModel::~PointTreeModel() 20 | { 21 | delete rootItem; 22 | } 23 | 24 | int PointTreeModel::columnCount(const QModelIndex & /* parent */) const 25 | { 26 | return rootItem->columnCount(); 27 | } 28 | 29 | QVariant PointTreeModel::data(const QModelIndex &index, int role) const 30 | { 31 | if (!index.isValid()) 32 | return QVariant(); 33 | 34 | if (role != Qt::DisplayRole && role != Qt::EditRole) 35 | return QVariant(); 36 | 37 | PointTreeItem *item = getItem(index); 38 | 39 | return item->data(index.column()); 40 | } 41 | 42 | Qt::ItemFlags PointTreeModel::flags(const QModelIndex &index) const 43 | { 44 | if (!index.isValid()) 45 | return 0; 46 | 47 | return Qt::ItemIsEditable | Qt::ItemIsEnabled | Qt::ItemIsSelectable; 48 | } 49 | 50 | PointTreeItem *PointTreeModel::getItem(const QModelIndex &index) const 51 | { 52 | if (index.isValid()) 53 | { 54 | PointTreeItem *item = static_cast< PointTreeItem * >(index.internalPointer()); 55 | if (item) 56 | return item; 57 | } 58 | return rootItem; 59 | } 60 | 61 | QVariant PointTreeModel::headerData(int section, Qt::Orientation orientation, int role) const 62 | { 63 | if (orientation == Qt::Horizontal && role == Qt::DisplayRole) 64 | return rootItem->data(section); 65 | 66 | return QVariant(); 67 | } 68 | 69 | QModelIndex PointTreeModel::index(int row, int column, const QModelIndex &parent) const 70 | { 71 | if (parent.isValid() && parent.column() != 0) 72 | return QModelIndex(); 73 | 74 | PointTreeItem *parentItem = getItem(parent); 75 | 76 | PointTreeItem *childItem = parentItem->child(row); 77 | if (childItem) 78 | return createIndex(row, column, childItem); 79 | else 80 | return QModelIndex(); 81 | } 82 | 83 | bool PointTreeModel::insertColumns(int position, int columns, const QModelIndex &parent) 84 | { 85 | bool success; 86 | 87 | beginInsertColumns(parent, position, position + columns - 1); 88 | success = rootItem->insertColumns(position, columns); 89 | endInsertColumns(); 90 | 91 | return success; 92 | } 93 | 94 | bool PointTreeModel::insertRows(int position, int rows, const QModelIndex &parent) 95 | { 96 | PointTreeItem *parentItem = getItem(parent); 97 | bool success; 98 | 99 | beginInsertRows(parent, position, position + rows - 1); 100 | success = parentItem->insertChildren(position, rows, rootItem->columnCount()); 101 | endInsertRows(); 102 | 103 | return success; 104 | } 105 | 106 | QModelIndex PointTreeModel::parent(const QModelIndex &index) const 107 | { 108 | if (!index.isValid()) 109 | return QModelIndex(); 110 | 111 | PointTreeItem *childItem = getItem(index); 112 | PointTreeItem *parentItem = childItem->parent(); 113 | 114 | if (parentItem == rootItem) 115 | return QModelIndex(); 116 | 117 | return createIndex(parentItem->childNumber(), 0, parentItem); 118 | } 119 | 120 | bool PointTreeModel::removeColumns(int position, int columns, const QModelIndex &parent) 121 | { 122 | bool success; 123 | 124 | beginRemoveColumns(parent, position, position + columns - 1); 125 | success = rootItem->removeColumns(position, columns); 126 | endRemoveColumns(); 127 | 128 | if (rootItem->columnCount() == 0) 129 | removeRows(0, rowCount()); 130 | 131 | return success; 132 | } 133 | 134 | bool PointTreeModel::removeRows(int position, int rows, const QModelIndex &parent) 135 | { 136 | PointTreeItem *parentItem = getItem(parent); 137 | bool success = true; 138 | 139 | beginRemoveRows(parent, position, position + rows - 1); 140 | success = parentItem->removeChildren(position, rows); 141 | endRemoveRows(); 142 | 143 | return success; 144 | } 145 | 146 | int PointTreeModel::rowCount(const QModelIndex &parent) const 147 | { 148 | PointTreeItem *parentItem = getItem(parent); 149 | 150 | return parentItem->childCount(); 151 | } 152 | 153 | bool PointTreeModel::setData(const QModelIndex &index, const QVariant &value, int role) 154 | { 155 | if (role != Qt::EditRole) 156 | return false; 157 | 158 | PointTreeItem *item = getItem(index); 159 | bool result = item->setData(index.column(), value); 160 | 161 | if (result) 162 | Q_EMIT dataChanged(index, index); 163 | 164 | return result; 165 | } 166 | 167 | bool PointTreeModel::setHeaderData(int section, Qt::Orientation orientation, const QVariant &value, int role) 168 | { 169 | if (role != Qt::EditRole || orientation != Qt::Horizontal) 170 | return false; 171 | 172 | bool result = rootItem->setData(section, value); 173 | 174 | if (result) 175 | Q_EMIT headerDataChanged(orientation, section, section); 176 | 177 | return result; 178 | } 179 | 180 | void PointTreeModel::setupModelData(const QStringList &lines, PointTreeItem *parent) 181 | { 182 | QList< PointTreeItem * > parents; 183 | QList< int > indentations; 184 | parents << parent; 185 | indentations << 0; 186 | 187 | int number = 0; 188 | 189 | while (number < lines.count()) 190 | { 191 | int position = 0; 192 | while (position < lines[number].length()) 193 | { 194 | if (lines[number].mid(position, 1) != " ") 195 | break; 196 | position++; 197 | } 198 | 199 | QString lineData = lines[number].mid(position).trimmed(); 200 | 201 | if (!lineData.isEmpty()) 202 | { 203 | // Read the column data from the rest of the line. 204 | QStringList columnStrings = lineData.split("\t", QString::SkipEmptyParts); 205 | QVector< QVariant > columnData; 206 | for (int column = 0; column < columnStrings.count(); ++column) 207 | columnData << columnStrings[column]; 208 | 209 | if (position > indentations.last()) 210 | { 211 | // The last child of the current parent is now the new parent 212 | // unless the current parent has no children. 213 | if (parents.last()->childCount() > 0) 214 | { 215 | parents << parents.last()->child(parents.last()->childCount() - 1); 216 | indentations << position; 217 | } 218 | } 219 | else 220 | { 221 | while (position < indentations.last() && parents.count() > 0) 222 | { 223 | parents.pop_back(); 224 | indentations.pop_back(); 225 | } 226 | } 227 | 228 | // Append a new item to the current parent's list of children. 229 | PointTreeItem *parent = parents.last(); 230 | parent->insertChildren(parent->childCount(), 1, rootItem->columnCount()); 231 | for (int column = 0; column < columnData.size(); ++column) 232 | parent->child(parent->childCount() - 1)->setData(column, columnData[column]); 233 | } 234 | 235 | number++; 236 | } 237 | } 238 | -------------------------------------------------------------------------------- /workspace_visualization/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 workspace_visualization 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 map_creator::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 workspace_visualization 229 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/widgets/base_placement_widget.h: -------------------------------------------------------------------------------- 1 | #ifndef base_placement_widget_H_ 2 | #define base_placement_widget_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | // macros 38 | #ifndef DEG2RAD 39 | #define DEG2RAD(x) ((x)*0.017453293) 40 | #endif 41 | 42 | #ifndef RAD2DEG 43 | #define RAD2DEG(x) ((x)*57.29578) 44 | #endif 45 | 46 | namespace base_placement_plugin 47 | { 48 | namespace widgets 49 | { 50 | /*! 51 | * \brief Class for handling the User Interactions with the RQT Widget. 52 | * \details The PathPlanningWidget Class handles all User Interactions with the RQT GUI. 53 | This Class inherits from the QWidget superclass. 54 | The concept of the RQT widget is to add the same possabilities as the UI from the RViz enviroment and enabling 55 | simultanious communication betweet the RViz Enviroment and the RQT GUI. 56 | * \author Abhijit Makhal 57 | */ 58 | 59 | class BasePlacementWidget : public QWidget 60 | { 61 | Q_OBJECT 62 | public: 63 | //! RQT Widget Constructor. 64 | BasePlacementWidget(std::string ns = ""); 65 | //! Virtual RQT Widget Destructor. 66 | virtual ~BasePlacementWidget(); 67 | //! set the name of the RQT Widget. 68 | std::string get_name() 69 | { 70 | return "BasePlacementPlanner"; 71 | } 72 | 73 | protected: 74 | //! Widget Initialization. 75 | void init(); 76 | std::string param_ns_; 77 | //! Protected variable for the Qt UI to access the Qt UI components. 78 | Ui::BasePlacementWidget ui_; 79 | //! Definition of an abstract data model. 80 | QStandardItemModel* pointDataModel; 81 | 82 | QObject* add_robot ; 83 | 84 | 85 | 86 | 87 | //! Bool value to show/unshow map 88 | bool show_union_map_; 89 | bool show_umodels_; 90 | std::string group_name_; 91 | 92 | private: 93 | QStringList pointList; 94 | //! Checks the range of the points. 95 | void pointRange(); 96 | protected Q_SLOTS: 97 | //! Initialize the TreeView with the User Interactive Marker. 98 | void initTreeView(); 99 | //! Handle the event of a Way-Point deleted from the RQT UI. 100 | void pointDeletedUI(); 101 | //! Handle the event of a Way-Point added from the RQT UI. 102 | void pointAddUI(); 103 | //! Insert a row in the TreeView. 104 | void insertRow(const tf::Transform& point_pos, const int count); 105 | //! Remove a row in the TreeView. 106 | void removeRow(int marker_nr); 107 | //! Handle the event when a User updates the pose of a Way-Point through the RQT UI. 108 | void pointPosUpdated_slot(const tf::Transform& point_pos, const char* marker_name); 109 | //! Get the selected Way-Point from the RQT UI. 110 | void selectedPoint(const QModelIndex& current, const QModelIndex& previous); 111 | //! Handle the even when the data in the TreeView has been changed. 112 | void treeViewDataChanged(const QModelIndex& index, const QModelIndex& index2); 113 | //! Slot for parsing the Way-Points and notifying the MoveIt. 114 | void parseWayPointBtn_slot(); 115 | //! Send a signal that a save the Way-Points to a file button has been pressed. 116 | void savePointsToFile(); 117 | //! Send a signal that a load the Way-Points from a file button has been pressed. 118 | void loadPointsFromFile(); 119 | 120 | //! Send a signal that a loading of reachability fiel is happening 121 | void loadReachabilityFile(); 122 | //! Send a signal if the user wants to show the union map 123 | void showUnionMapFromUI(); 124 | //! Send a signal to clear the union map 125 | void clearUnionMapFromUI(); 126 | 127 | //! Send a signal if the user wants to start base placement by intution 128 | void startUserIntution(); 129 | 130 | //!receive the base base waypoints and send them to place base 131 | void getWaypoints(std::vector base_poses); 132 | 133 | 134 | 135 | 136 | //! Slot connected to a clear all points button click. 137 | void clearAllPoints_slot(); 138 | //! Set the start pose of the User Interactive Marker to correspond to the loaded robot base frame. 139 | void setAddPointUIStartPos(const tf::Transform end_effector); 140 | //! Slot for disabling the TabWidged while Base Placement is executed. 141 | void PlaceBaseStartedHandler(); 142 | //! Slot for enabling the TabWidged after Base Placement is executed. 143 | void PlaceBaseFinishedHandler(); 144 | 145 | //! Sending base placement parametes 146 | void sendBasePlacementParamsFromUI(); 147 | //! Set a message showing the process is completed 148 | void PlaceBaseCompleted_slot(double score); 149 | 150 | //! Set the Method name ComboBox 151 | void getBasePlacePlanMethod(std::vector< std::string > methods); 152 | //! Set the ouput type name in the ComboBox 153 | void getOutputType(std::vector< std::string > op_types); 154 | //! Set the ouput type name in the ComboBox 155 | void getRobotGroups(std::vector< std::string > groups); 156 | //! Get selected group from place_base 157 | void getSelectedGroup(std::string group_name); 158 | 159 | void selectedMethod(int index); 160 | void selectedOuputType(int op_index); 161 | void selectedRobotGroup(int index); 162 | void showUreachModels(); 163 | 164 | Q_SIGNALS: 165 | 166 | //! Signal to send reachability data after loading from file 167 | void reachabilityData_signal(std::multimap< std::vector< double >, std::vector< double > > PoseColFilter, 168 | std::multimap< std::vector< double >, double > SphereCol, float res); 169 | // Signal bool to show union map 170 | void showUnionMap_signal(bool show_union_map_); 171 | // Signal bool to clear union map 172 | void clearUnionMap_signal(bool show_union_map_); 173 | 174 | //! Notify RViz enviroment that a new Way-Point has been added from RQT. 175 | void addPoint(const tf::Transform point_pos); 176 | //! Notify RViz enviroment that a new Way-Point has been deleted from RQT. 177 | void pointDelUI_signal(std::string marker_name); 178 | //! Notify RViz enviroment that a new Way-Point has been modified from RQT. 179 | void pointPosUpdated_signal(const tf::Transform& position, const char* marker_name); 180 | //! Signal to notify the base placement planner that the user has pressed the find base button 181 | void parseWayPointBtn_signal(); 182 | //! Save to file button has been pressed. 183 | void saveToFileBtn_press(); 184 | //! Signal that clear all points button has been pressed. 185 | void clearAllPoints_signal(); 186 | //! Signal to send base placement parametes 187 | void basePlacementParamsFromUI_signal(int base_loc_size_, int high_socre_sp_); 188 | //! Sending selected method 189 | void SendSelectedMethod(int index); 190 | //! Sending selected ouput type 191 | void SendSelectedOpType(int op_index); 192 | //! Sending selected robot group 193 | void SendSelectedRobotGroup(int index); 194 | //! Sending selected umodel showing method 195 | void SendShowUmodel(bool umodel); 196 | //!Sending baseposes selected by user 197 | void SendBasePoses(std::vector); 198 | 199 | 200 | }; 201 | } 202 | 203 | } // end of namespace base_placement_plugin 204 | 205 | #endif // base_placement_widget_H_ 206 | -------------------------------------------------------------------------------- /base_placement_plugin/include/base_placement_plugin/place_base.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #ifndef PLACE_BASE_H_ 26 | #define PLACE_BASE_H_ 27 | 28 | /*! 29 | * \brief Class for setting up the Base Placement Environment. 30 | * \details The PlaceBase Class handles all the interactions between the widget and the actual task. 31 | This Class inherits from the QObject superclass. 32 | The concept of this class is to initialize all the necessary parameters for generating Optimal Base Locations 33 | from task poses. 34 | 35 | * \author Abhijit Makhal 36 | */ 37 | 38 | class PlaceBase : public QObject 39 | { 40 | Q_OBJECT 41 | 42 | public: 43 | public: 44 | //! Constructor for the Base Placement Planner. 45 | PlaceBase(QObject* parent = 0); 46 | //! Virtual Destructor for the Base Placement planner. 47 | virtual ~PlaceBase(); 48 | //! Initialization of the necessary parameters. 49 | void init(); 50 | 51 | public Q_SLOTS: 52 | //!Getting the reachability data from widget 53 | void setReachabilityData(std::multimap< std::vector< double >, std::vector< double > > PoseCollection, 54 | std::multimap< std::vector< double >, double > SphereCollection, float resolution); 55 | 56 | //!Showing the InverseReachability Map 57 | void ShowUnionMap(bool show_map); 58 | 59 | //!Clearing the InverseReachability Map 60 | void clearUnionMap(bool clear_map); 61 | 62 | //!Showing the base locations by arrow 63 | void showBaseLocationsbyArrow(std::vector< geometry_msgs::Pose > po); 64 | 65 | //! Showing the base locations by robot model 66 | void showBaseLocationsbyRobotModel(std::vector< geometry_msgs::Pose > po); 67 | 68 | //! Showing the base locations by only arm 69 | void showBaseLocationsbyArmModel(std::vector< geometry_msgs::Pose > po); 70 | 71 | //! Get the Way-Points from the RViz enviroment and use them to find base. 72 | bool findbase(std::vector< geometry_msgs::Pose > grasp_poses); 73 | 74 | //! Sending the initial marker location and list of base Placement Method and visualization methods 75 | void initRvizDone(); 76 | 77 | //! Function for setting base placement mainfunction to a separate thread. 78 | void BasePlacementHandler(std::vector< geometry_msgs::Pose > waypoints); 79 | 80 | //! Setting the Base Placement Paremeters 81 | void setBasePlaceParams(int base_loc_size, int high_score_sp_); 82 | 83 | //! Getting user Defined base placement method 84 | void getSelectedMethod(int index); 85 | 86 | //! Getting user Defined visualization method 87 | void getSelectedOpType(int op_index); 88 | 89 | //! Getting user Defined robot model 90 | void getSelectedRobotGroup(int model_index); 91 | 92 | //! Getting user Defined unreachable model shown method 93 | void getShowUreachModels(bool show_u_models); 94 | 95 | //!Getting the user Defined base poses 96 | void getBasePoses(std::vector base_poses); 97 | 98 | 99 | 100 | Q_SIGNALS: 101 | 102 | //! Signal for initial marker frame 103 | void getinitialmarkerFrame_signal(const tf::Transform trns); 104 | 105 | //! Let the RQT Widget know that a Base Placement Process has started. 106 | void basePlacementProcessStarted(); 107 | 108 | 109 | //! Let the RQT Widget know that Base Placement Process has finished. 110 | void basePlacementProcessFinished(); 111 | 112 | //! Let the RQT Widget know that Base Placement Process completed so it can show finish message 113 | void basePlacementProcessCompleted(double score); 114 | 115 | //! Send the Method groups to the GUI 116 | void sendBasePlaceMethods_signal(std::vector< std::string > method_names); 117 | 118 | //! Send the output types to the GUI 119 | void sendOuputType_signal(std::vector< std::string > output_type); 120 | 121 | //! Send the output types to the GUI 122 | void sendGroupType_signal(std::vector< std::string > group_names); 123 | 124 | //! Send the selected groupd 125 | void sendSelectedGroup_signal(std::string group_name); 126 | 127 | 128 | 129 | protected: 130 | //QObject* add_robot; 131 | //AddRobotBase add_robot; 132 | //! Base Placement by PCA 133 | void findBaseByPCA(); 134 | 135 | //! Base Placement by GraspReachabilityScore 136 | void findBaseByGraspReachabilityScore(); 137 | 138 | //!Base Placement by IKSolutionScore 139 | void findBaseByIKSolutionScore(); 140 | 141 | //!Base Placement by VerticalRobotModel 142 | void findBaseByVerticalRobotModel(); 143 | 144 | //!Base Placement by VerticalRobotModel 145 | void findBaseByUserIntuition(); 146 | 147 | //! Selecting the Base Placement Method 148 | void BasePlaceMethodHandler(); 149 | 150 | //! Selecting the Visualization Method 151 | void OuputputVizHandler(std::vector< geometry_msgs::Pose > po); 152 | 153 | 154 | 155 | //Transforming reachability data towards robot base 156 | void transformToRobotbase(std::multimap< std::vector< double >, std::vector< double > > armBasePoses, 157 | std::multimap< std::vector< double >, std::vector< double > >& robotBasePoses); 158 | 159 | void transformFromRobotbaseToArmBase(const geometry_msgs::Pose& base_pose, geometry_msgs::Pose &arm_base_pose); 160 | void createSpheres(std::multimap< std::vector< double >, std::vector< double > > basePoses, 161 | std::map< std::vector< double >, double >& spColor, std::vector< std::vector< double > >& highScoredSp, bool reduce_D); 162 | 163 | double calculateScoreForRobotBase(std::vector& grasp_poses, std::vector& base_poses); 164 | double calculateScoreForArmBase(std::vector& grasp_poses, std::vector& base_poses); 165 | 166 | bool loadRobotModel(); 167 | bool checkforRobotModel(); 168 | 169 | void getRobotGroups(std::vector& groups); 170 | 171 | int selected_method_; 172 | int selected_op_type_; 173 | std::string selected_group_; 174 | 175 | //! Number of base place locations to show 176 | int BASE_LOC_SIZE_; 177 | //! Number of High Scoring Spheres 178 | int HIGH_SCORE_SP_; 179 | 180 | //! Vector for method_names 181 | std::vector< std::string > method_names_; 182 | //! Vector for output visualization 183 | std::vector< std::string > output_type_; 184 | //! Vector for robot groups 185 | std::vector group_names_; 186 | //! Taking the reachability data from the file 187 | std::multimap< std::vector< double >, std::vector< double > > PoseColFilter; 188 | std::multimap< std::vector< double >, double > SphereCol; 189 | float res; 190 | 191 | //! Reachability data for the union map 192 | std::multimap< std::vector< double >, std::vector< double > > baseTrnsCol; 193 | std::map< std::vector< double >, double > sphereColor; 194 | 195 | //! sphere centers based on their scores 196 | std::vector< std::vector< double > > highScoreSp; 197 | //! Vector for storing final base poses 198 | std::vector< geometry_msgs::Pose > final_base_poses; 199 | std::vector final_base_poses_user; 200 | 201 | //! Vector for grasp poses 202 | std::vector< geometry_msgs::Pose > GRASP_POSES_; 203 | 204 | 205 | //! Reachability data for the union map 206 | std::multimap< std::vector< double >, std::vector< double > > robot_PoseColfilter; 207 | 208 | double score_; 209 | geometry_msgs::Pose best_pose_; 210 | 211 | //Robot model cons pointer 212 | moveit::core::RobotModelConstPtr robot_model_; 213 | 214 | //show unreachable models 215 | bool show_ureach_models_; 216 | 217 | //Pointer for robot marker 218 | CreateMarker* mark_; 219 | }; 220 | 221 | #endif // PLACE_BASE_H_ 222 | -------------------------------------------------------------------------------- /map_creator/src/create_reachability_map.cpp: -------------------------------------------------------------------------------- 1 | // The spheres and poses are fused in a single dataset, instead of two datasets for sphere and poses 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "geometry_msgs/PoseArray.h" 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | //struct stat st; 19 | 20 | typedef std::vector, const std::vector< double >* > > MultiVector; 21 | //typedef std::multimap< const std::vector< double >*, const std::vector< double >* > MultiMap; 22 | 23 | bool isFloat(std::string s) 24 | { 25 | std::istringstream iss(s); 26 | float dummy; 27 | iss >> std::noskipws >> dummy; 28 | return iss && iss.eof(); // Result converted to bool 29 | } 30 | 31 | int main(int argc, char **argv) 32 | { 33 | ros::init(argc, argv, "workspace"); 34 | ros::NodeHandle n; 35 | ros::Time startit = ros::Time::now(); 36 | float resolution = 0.08; 37 | kinematics::Kinematics k; 38 | std::string file = str(boost::format("%s_r%d_reachability.h5") % k.getRobotName() % resolution); 39 | std::string path(ros::package::getPath("map_creator") + "/maps/"); 40 | std::string filename; 41 | if (argc == 2) 42 | { 43 | if (!isFloat(argv[1])) 44 | { 45 | ROS_ERROR_STREAM("Probably you have just provided only the map filename. Hey!! The first argument is the " 46 | "resolution."); 47 | return 0; 48 | } 49 | resolution = atof(argv[1]); 50 | file = str(boost::format("%s_r%d_reachability.h5") % k.getRobotName() % resolution); 51 | filename = path + file; 52 | } 53 | 54 | else if (argc == 3) 55 | { 56 | std::string name; 57 | name = argv[2]; 58 | if (!isFloat(argv[1]) && isFloat(argv[2])) 59 | { 60 | ROS_ERROR_STREAM("Hey!! The first argument is the resolution and the second argument is the map filename. You " 61 | "messed up."); 62 | return 0; 63 | } 64 | 65 | else 66 | { 67 | resolution = atof(argv[1]); 68 | std::string str(argv[2]); 69 | if(std::strchr(str.c_str(), '/')) 70 | { 71 | filename = argv[2]; 72 | } 73 | else 74 | filename = path + str; 75 | } 76 | } 77 | else if (argc < 2) 78 | { 79 | ROS_INFO("You have not provided any argument. So taking default values."); 80 | filename = path + file; 81 | } 82 | // ros::Publisher workspace_pub = n.advertise("workspace", 10); 83 | ros::Rate loop_rate(10); 84 | 85 | int count = 0; 86 | 87 | while (ros::ok()) 88 | { 89 | unsigned char max_depth = 16; 90 | unsigned char minDepth = 0; 91 | 92 | // A box of radius 1 is created. It will be the size of the robot+1.5. Then the box is discretized by voxels of 93 | // specified resolution 94 | // TODO resolution will be user argument 95 | // The center of every voxels are stored in a vector 96 | 97 | sphere_discretization::SphereDiscretization sd; 98 | float r = 1; 99 | octomap::point3d origin = octomap::point3d(0, 0, 0); // This point will be the base of the robot 100 | octomap::OcTree *tree = sd.generateBoxTree(origin, r, resolution); 101 | std::vector< octomap::point3d > new_data; 102 | ROS_INFO("Creating the box and discretizing with resolution: %f", resolution); 103 | int sphere_count = 0; 104 | for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(max_depth), end = tree->end_leafs(); it != end; ++it) 105 | { 106 | sphere_count++; 107 | } 108 | new_data.reserve(sphere_count); 109 | for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(max_depth), end = tree->end_leafs(); it != end; ++it) 110 | { 111 | new_data.push_back(it.getCoordinate()); 112 | } 113 | 114 | ROS_INFO("Total no of spheres now: %lu", new_data.size()); 115 | ROS_INFO("Please hold ON. Spheres are discretized and all of the poses are checked for Ik solutions. May take some " 116 | "time"); 117 | 118 | // A sphere is created in every voxel. The sphere may be created by default or other techniques. 119 | // TODO Other techniques need to be modified. the user can specifiy which technique they want to use 120 | // TODO The sphere discretization parameter and rotation of every poses will be taken as argument. If the final 121 | // joints can rotate (0, 2pi) we dont need to rotate the poses. 122 | // Every discretized points on spheres are converted to pose and all the poses are saved in a multimap with their 123 | // corresponding sphere centers 124 | // If the resolution is 0.01 the programs not responds 125 | 126 | float radius = resolution; 127 | 128 | VectorOfVectors sphere_coord; 129 | sphere_coord.resize( new_data.size() ); 130 | 131 | MultiVector pose_col; 132 | pose_col.reserve( new_data.size() * 50); 133 | 134 | for (int i = 0; i < new_data.size(); i++) 135 | { 136 | static std::vector< geometry_msgs::Pose > pose; 137 | sd.convertPointToVector(new_data[i], sphere_coord[i]); 138 | 139 | sd.make_sphere_poses(new_data[i], radius, pose); 140 | for (int j = 0; j < pose.size(); j++) 141 | { 142 | static std::vector< double > point_on_sphere; 143 | sd.convertPoseToVector(pose[j], point_on_sphere); 144 | pose_col.push_back( std::make_pair(point_on_sphere, &sphere_coord[i])); 145 | } 146 | } 147 | 148 | // Every pose is checked for IK solutions. The reachable poses and the their corresponsing joint solutions are 149 | // stored. Only the First joint solution is stored. We may need this solutions in the future. Otherwise we can show 150 | // the robot dancing with the joint solutions in a parallel thread 151 | // TODO Support for more than 6DOF robots needs to be implemented. 152 | 153 | // Kinematics k; 154 | 155 | MultiMapPtr pose_col_filter; 156 | VectorOfVectors ik_solutions; 157 | ik_solutions.reserve( pose_col.size() ); 158 | 159 | for (MultiVector::iterator it = pose_col.begin(); it != pose_col.end(); ++it) 160 | { 161 | static std::vector< double > joints(6); 162 | int solns; 163 | if (k.isIKSuccess(it->first, joints, solns)) 164 | { 165 | pose_col_filter.insert( std::make_pair( it->second, &(it->first))); 166 | ik_solutions.push_back(joints); 167 | // cout<first[0]<<" "<first[1]<<" "<first[2]<<" "<first[3]<<" "<first[4]<<" 168 | // "<first[5]<<" "<first[6]<* sphere_coord = it->first; 186 | //const std::vector* point_on_sphere = it->second; 187 | 188 | // Reachability Index D=R/N*100; 189 | float d = float(pose_col_filter.count(sphere_coord)) / (pose_col.size() / new_data.size()) * 100; 190 | sphere_color.insert( std::make_pair(it->first, double(d))); 191 | } 192 | 193 | ROS_INFO("No of spheres reachable: %lu", sphere_color.size()); 194 | 195 | // Creating maps now 196 | //Saving map to dataset 197 | hdf5_dataset::Hdf5Dataset h5(filename); 198 | h5.saveReachMapsToDataset(pose_col_filter, sphere_color, resolution); 199 | 200 | double dif = ros::Duration( ros::Time::now() - startit).toSec(); 201 | ROS_INFO("Elasped time is %.2lf seconds.", dif); 202 | ROS_INFO("Completed"); 203 | ros::spinOnce(); 204 | // sleep(10000); 205 | return 1; 206 | loop_rate.sleep(); 207 | count; 208 | } 209 | return 0; 210 | } 211 | -------------------------------------------------------------------------------- /base_placement_plugin/src/create_marker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | double unifRand() 6 | { 7 | return rand() / double(RAND_MAX); 8 | } 9 | 10 | CreateMarker::CreateMarker(std::string group_name) : spinner(1), group_name_(group_name) 11 | { 12 | spinner.start(); 13 | group_.reset(new moveit::planning_interface::MoveGroup(group_name_)); 14 | //ROS_INFO_STREAM("Selected planning group: "<< group_->getName()); 15 | robot_model_ = group_->getRobotModel(); 16 | } 17 | 18 | bool CreateMarker::checkEndEffector() 19 | { 20 | if(!(group_->getEndEffector()).empty()) 21 | return true; 22 | else 23 | return false; 24 | } 25 | 26 | void CreateMarker::discardUnreachableModels(BasePoseJoint& baseJoints) 27 | { 28 | for(BasePoseJoint::iterator it= baseJoints.begin(); it !=baseJoints.end();) 29 | { 30 | std::vector joint_soln = it->first; 31 | if(std::equal(joint_soln.begin()+1, joint_soln.end(), joint_soln.begin())) 32 | { 33 | BasePoseJoint::iterator save = it; 34 | ++save; 35 | baseJoints.erase(it); 36 | it = save; 37 | } 38 | else 39 | ++it; 40 | } 41 | } 42 | 43 | bool checkForJointSoln(const std::vector& soln) 44 | { 45 | if(std::equal(soln.begin()+1, soln.end(), soln.begin())) 46 | return true; 47 | } 48 | 49 | void CreateMarker::updateRobotState(const std::vector& joint_soln, moveit::core::RobotStatePtr robot_state) 50 | { 51 | std::string robot_name = group_->getName(); 52 | const moveit::core::JointModelGroup* robot_jmp = robot_model_->getJointModelGroup(robot_name); 53 | std::vector joint_names = robot_jmp->getActiveJointModelNames(); 54 | for (int i=0;isetJointPositions(joint_names[i], &(joint_soln[i])); 57 | } 58 | robot_state->update(); 59 | } 60 | 61 | void CreateMarker::getArmLinks(std::vector& arm_links) 62 | { 63 | std::string arm_name = group_->getName(); 64 | const moveit::core::JointModelGroup* arm_jmp = robot_model_->getJointModelGroup(arm_name); 65 | arm_links = arm_jmp->getLinkModelNames(); 66 | } 67 | 68 | void CreateMarker::getEELinks(std::vector& ee_links) 69 | { 70 | std::string ee_name = group_->getEndEffector(); 71 | if(robot_model_->hasJointModelGroup(ee_name)) 72 | { 73 | const moveit::core::JointModelGroup* ee_jmp = robot_model_->getJointModelGroup(ee_name); 74 | ee_links = ee_jmp->getLinkModelNames(); 75 | } 76 | } 77 | 78 | void CreateMarker::getFullLinkNames(std::vector& full_link_names, bool arm_only) 79 | { 80 | std::vector full_links = robot_model_->getLinkModelNames(); 81 | if(arm_only) 82 | { 83 | std::vector arm_links, ee_links; 84 | getArmLinks(arm_links); 85 | getEELinks(ee_links); 86 | int position = std::find(full_links.begin(), full_links.end(), arm_links[0]) - full_links.begin(); 87 | parent_link = full_links[position -1]; 88 | for(int i=0;i& joint_soln,bool arm_only, bool is_reachable, visualization_msgs::InteractiveMarkerControl& robotModelControl) 141 | { 142 | moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model_)); 143 | updateRobotState(joint_soln, robot_state); 144 | std::vector full_link_names; 145 | getFullLinkNames(full_link_names, arm_only); 146 | visualization_msgs::MarkerArray full_link_markers; 147 | robot_state->getRobotMarkers(full_link_markers, full_link_names); 148 | 149 | //const std::string& parent_link = full_link_names[0]; //root link 150 | Eigen::Affine3d tf_root_to_first_link = robot_state->getGlobalLinkTransform(parent_link); 151 | Eigen::Affine3d tf_first_link_to_root = tf_root_to_first_link.inverse(); 152 | 153 | updateMarkers(base_pose, is_reachable, tf_first_link_to_root, full_link_markers); 154 | for(int i=0;i& joint_soln, 162 | const int& num, bool arm_only, bool is_reachable,visualization_msgs::InteractiveMarker& iMarker) 163 | { 164 | iMarker.header.frame_id = "base_link"; 165 | iMarker.pose = base_pose; 166 | iMarker.scale = 0.2; 167 | std::string name = "robot_model"; 168 | std::string description = "robot_desc"; 169 | iMarker.name = name + boost::lexical_cast(num); 170 | iMarker.description = description+boost::lexical_cast(num); 171 | visualization_msgs::InteractiveMarkerControl robotModelControl; 172 | makeIntMarkerControl(base_pose, joint_soln, arm_only, is_reachable,robotModelControl); 173 | iMarker.controls.push_back(robotModelControl); 174 | } 175 | 176 | void CreateMarker::makeIntMarkers(BasePoseJoint &basePJoints, bool arm_only, std::vector &iMarkers) 177 | { 178 | iMarkers.clear(); 179 | for(BasePoseJoint::iterator it = basePJoints.begin(); it !=basePJoints.end();++it) 180 | { 181 | int i = std::distance(basePJoints.begin(), it); 182 | bool is_reachable = true; 183 | geometry_msgs::Pose base_pose = it->second; 184 | std::vector joint_soln = it->first; 185 | if(checkForJointSoln(joint_soln)) 186 | is_reachable = false; 187 | visualization_msgs::InteractiveMarker iMarker; 188 | createInteractiveMarker(base_pose, joint_soln, i, arm_only, is_reachable, iMarker ); 189 | iMarkers.push_back(iMarker); 190 | } 191 | } 192 | 193 | 194 | bool CreateMarker::makeRobotMarker(BasePoseJoint baseJoints, std::vector &iMarkers, bool show_unreachable_models) 195 | { 196 | if(!show_unreachable_models) 197 | discardUnreachableModels(baseJoints); 198 | makeIntMarkers(baseJoints, false, iMarkers); 199 | } 200 | 201 | 202 | bool CreateMarker::makeArmMarker(BasePoseJoint baseJoints, std::vector &iMarkers, bool show_unreachable_models) 203 | { 204 | if(!show_unreachable_models) 205 | discardUnreachableModels(baseJoints); 206 | makeIntMarkers(baseJoints, true, iMarkers); 207 | } 208 | 209 | 210 | visualization_msgs::MarkerArray CreateMarker::getDefaultMarkers() 211 | { 212 | moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model_)); 213 | //updateRobotState(joint_soln, robot_state); 214 | std::vector full_link_names; 215 | getFullLinkNames(full_link_names, false); 216 | visualization_msgs::MarkerArray full_link_markers; 217 | robot_state->getRobotMarkers(full_link_markers, full_link_names); 218 | return full_link_markers; 219 | } 220 | 221 | 222 | 223 | -------------------------------------------------------------------------------- /map_creator/src/create_inverse_reachability_map.cpp: -------------------------------------------------------------------------------- 1 | // The inverse reachability map depends on the reachability map. It is an inversion of the poses to the base location 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include "map_creator/WorkSpace.h" 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | //struct stat st; 29 | 30 | int main(int argc, char **argv) 31 | { 32 | ros::init(argc, argv, "inverse_workspace"); 33 | ros::NodeHandle n; 34 | // ros::Publisher workspace_pub = n.advertise("reachability_map", 1); 35 | time_t startit, finish; 36 | time(&startit); 37 | kinematics::Kinematics k; 38 | std::string file; 39 | std::string path(ros::package::getPath("map_creator") + "/maps/"); 40 | std::string filename; 41 | const char *input_FILE; 42 | 43 | if (argc < 2) 44 | { 45 | ROS_ERROR_STREAM("Please provide the name of the reachability map. If you have not created it yet, Please create " 46 | "the map by running the create reachability map node in map_creator package"); 47 | return 0; 48 | } 49 | 50 | else if (argc == 2) 51 | { 52 | ROS_INFO("Creating map with default name."); 53 | input_FILE = argv[1]; 54 | if(!boost::filesystem::exists(input_FILE)) 55 | { 56 | ROS_ERROR("Input file does not exist"); 57 | return false; 58 | } 59 | else 60 | { 61 | float res; 62 | hdf5_dataset::Hdf5Dataset h5_res(argv[1]); 63 | h5_res.open(); 64 | h5_res.h5ToResolution(res); 65 | h5_res.close(); 66 | file = str(boost::format("%s_r%d_Inv_reachability.h5") % k.getRobotName() % res); 67 | filename = path + file; 68 | } 69 | } 70 | 71 | else if (argc == 3) 72 | { 73 | input_FILE = argv[1]; 74 | std::string str(argv[2]); 75 | if(!boost::filesystem::exists(input_FILE)) 76 | { 77 | ROS_ERROR("Input file does not exist"); 78 | return false; 79 | } 80 | else 81 | { 82 | if(std::strchr(str.c_str(), '/')) 83 | { 84 | filename = argv[2]; 85 | } 86 | else 87 | filename = path + str; 88 | } 89 | } 90 | 91 | ros::Rate loop_rate(10); 92 | 93 | int count = 0; 94 | while (ros::ok()) 95 | { 96 | MultiMapPtr pose_col_filter; 97 | MapVecDoublePtr sphere_col; 98 | float res; 99 | 100 | hdf5_dataset::Hdf5Dataset h5file(input_FILE); 101 | h5file.open(); 102 | h5file.loadMapsFromDataset(pose_col_filter, sphere_col, res); 103 | 104 | // Starting to create the Inverse Reachability map. The resolution will be the same as the reachability map 105 | 106 | unsigned char max_depth = 16; 107 | unsigned char minDepth = 0; 108 | float size_of_box = 1.5; 109 | float resolution = res; 110 | sphere_discretization::SphereDiscretization sd; 111 | 112 | octomap::point3d origin = octomap::point3d(0, 0, 0); // As these map is independent of any task points, it is centered around origin. 113 | // For dependent maps, the whole map will be transformed to that certain task 114 | // point 115 | octomap::OcTree *tree = sd.generateBoxTree(origin, size_of_box, resolution); 116 | std::vector< octomap::point3d > new_data; 117 | 118 | std::vector< geometry_msgs::Pose > pose; 119 | sd.make_sphere_poses(origin, resolution, pose); // calculating number of points on a sphere by discretization 120 | 121 | for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(max_depth), end = tree->end_leafs(); it != end; ++it) 122 | { 123 | new_data.push_back(it.getCoordinate()); 124 | } 125 | 126 | ROS_INFO("Number of poses in RM: %lu", pose_col_filter.size()); 127 | 128 | ROS_INFO("Number of voxels: %lu", new_data.size()); 129 | 130 | /// All the poses are transformed in transformation matrices. For all the transforms, the translation part is 131 | // extracted and compared with voxel centers by Nighbors within voxel search 132 | 133 | pcl::PointCloud< pcl::PointXYZ >::Ptr cloud(new pcl::PointCloud< pcl::PointXYZ >); 134 | 135 | //TODO: take this trns_col multimap as typedef multimap with pointers and use in searching 136 | std::multimap< std::vector< float >, std::vector< float > > trns_col; 137 | for(MultiMapPtr::iterator it = pose_col_filter.begin(); it!= pose_col_filter.end(); ++it) 138 | { 139 | tf2::Vector3 vec((*it->second)[0], (*it->second)[1], (*it->second)[2]); 140 | tf2::Quaternion quat((*it->second)[3], (*it->second)[4], (*it->second)[5], (*it->second)[6]); 141 | tf2::Transform trns; 142 | trns.setOrigin(vec); 143 | trns.setRotation(quat); 144 | tf2::Transform trns_inv; 145 | trns_inv = trns.inverse(); 146 | 147 | tf2::Vector3 inv_trans_vec; 148 | tf2::Quaternion inv_trans_quat; 149 | inv_trans_vec = trns_inv.getOrigin(); 150 | inv_trans_quat = trns_inv.getRotation(); 151 | inv_trans_quat.normalize(); 152 | 153 | std::vector< float > position; 154 | position.push_back(inv_trans_vec[0]); 155 | position.push_back(inv_trans_vec[1]); 156 | position.push_back(inv_trans_vec[2]); 157 | std::vector< float > orientation; 158 | orientation.push_back(inv_trans_quat[0]); 159 | orientation.push_back(inv_trans_quat[1]); 160 | orientation.push_back(inv_trans_quat[2]); 161 | orientation.push_back(inv_trans_quat[3]); 162 | 163 | trns_col.insert(std::pair< std::vector< float >, std::vector< float > >(position, orientation)); 164 | 165 | pcl::PointXYZ point; 166 | point.x = inv_trans_vec[0]; 167 | point.y = inv_trans_vec[1]; 168 | point.z = inv_trans_vec[2]; 169 | cloud->push_back(point); 170 | 171 | } 172 | 173 | MultiMapPtr base_trns_col; 174 | pcl::octree::OctreePointCloudSearch< pcl::PointXYZ > octree(resolution); 175 | octree.setInputCloud(cloud); 176 | octree.addPointsFromInputCloud(); 177 | for (int i = 0; i < new_data.size(); i++) 178 | { 179 | pcl::PointXYZ search_point; 180 | search_point.x = new_data[i].x(); 181 | search_point.y = new_data[i].y(); 182 | search_point.z = new_data[i].z(); 183 | 184 | // Neighbors within voxel search 185 | 186 | std::vector< int > point_idx_vec; 187 | octree.voxelSearch(search_point, point_idx_vec); 188 | 189 | if (point_idx_vec.size() > 0) 190 | { 191 | 192 | std::vector< double >* base_sphere = new std::vector(); 193 | base_sphere->push_back(search_point.x); 194 | base_sphere->push_back(search_point.y); 195 | base_sphere->push_back(search_point.z); 196 | 197 | for (size_t j = 0; j < point_idx_vec.size(); ++j) 198 | { 199 | std::vector< float > base_pos; 200 | base_pos.push_back(cloud->points[point_idx_vec[j]].x); 201 | base_pos.push_back(cloud->points[point_idx_vec[j]].y); 202 | base_pos.push_back(cloud->points[point_idx_vec[j]].z); 203 | 204 | std::multimap< std::vector< float >, std::vector< float > >::iterator it1; 205 | 206 | for (it1 = trns_col.lower_bound(base_pos); it1 != trns_col.upper_bound(base_pos); ++it1) 207 | { 208 | std::vector< double >* base_pose = new std::vector(); 209 | base_pose->push_back(base_pos[0]); 210 | base_pose->push_back(base_pos[1]); 211 | base_pose->push_back(base_pos[2]); 212 | base_pose->push_back(it1->second[0]); 213 | base_pose->push_back(it1->second[1]); 214 | base_pose->push_back(it1->second[2]); 215 | base_pose->push_back(it1->second[3]); 216 | 217 | base_trns_col.insert(std::make_pair(base_sphere, base_pose)); 218 | } 219 | } 220 | } 221 | } 222 | 223 | MapVecDoublePtr sphere_color; 224 | for (MultiMapPtr::iterator it = base_trns_col.begin(); it!=base_trns_col.end(); ++it) 225 | { 226 | const std::vector* sphere_coord = it->first; 227 | float d = (float(base_trns_col.count(sphere_coord)) / pose.size()) * 100; 228 | sphere_color.insert( std::make_pair(it->first, double(d))); 229 | } 230 | 231 | ROS_INFO("Numer of Spheres in RM: %lu", sphere_col.size()); 232 | ROS_INFO("Numer of Spheres in IRM: %lu", sphere_color.size()); 233 | 234 | ROS_INFO("All the poses have Processed. Now saving data to a inverse Reachability Map."); 235 | 236 | hdf5_dataset::Hdf5Dataset irm_h5(filename); 237 | irm_h5.saveReachMapsToDataset(base_trns_col, sphere_color, res); 238 | 239 | 240 | time(&finish); 241 | double dif = difftime(finish, startit); 242 | ROS_INFO("Elasped time is %.2lf seconds.", dif); 243 | ROS_INFO("Completed"); 244 | ros::spinOnce(); 245 | // sleep(10000); 246 | return 1; 247 | loop_rate.sleep(); 248 | count; 249 | } 250 | 251 | return 0; 252 | } 253 | -------------------------------------------------------------------------------- /map_creator/src/kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #if IK_VERSION > 54 4 | #define IKREAL_TYPE IkReal // for IKFast 56,61 5 | #else 6 | #define IKREAL_TYPE IKReal // for IKFast 54 7 | #endif 8 | 9 | IKREAL_TYPE eerot[9], eetrans[3]; 10 | namespace kinematics 11 | { 12 | float Kinematics::SIGN(float x) 13 | { 14 | return (x >= 0.0f) ? +1.0f : -1.0f; 15 | } 16 | 17 | float Kinematics::NORM(float a, float b, float c, float d) 18 | { 19 | return sqrt(a * a + b * b + c * c + d * d); 20 | } 21 | 22 | void Kinematics::getPoseFromFK(const std::vector< double > joint_values, std::vector< double >& pose) 23 | { 24 | #if IK_VERSION > 54 25 | // for IKFast 56,61 26 | unsigned int num_of_joints = GetNumJoints(); 27 | unsigned int num_free_parameters = GetNumFreeParameters(); 28 | #else 29 | // for IKFast 54 30 | unsigned int num_of_joints = getNumJoints(); 31 | unsigned int num_free_parameters = getNumFreeParameters(); 32 | #endif 33 | IKREAL_TYPE joints[num_of_joints]; 34 | 35 | // cout< 54 42 | // for IKFast 56,61 43 | ComputeFk(joints, eetrans, eerot); // void return 44 | #else 45 | // for IKFast 54 46 | fk(joints, eetrans, eerot); // void return 47 | #endif 48 | // cout<<"translation: "<= q1 && q0 >= q2 && q0 >= q3) 67 | { 68 | q0 *= +1.0f; 69 | q1 *= SIGN(eerot[7] - eerot[5]); 70 | q2 *= SIGN(eerot[2] - eerot[6]); 71 | q3 *= SIGN(eerot[3] - eerot[1]); 72 | } 73 | else if (q1 >= q0 && q1 >= q2 && q1 >= q3) 74 | { 75 | q0 *= SIGN(eerot[7] - eerot[5]); 76 | q1 *= +1.0f; 77 | q2 *= SIGN(eerot[3] + eerot[1]); 78 | q3 *= SIGN(eerot[2] + eerot[6]); 79 | } 80 | else if (q2 >= q0 && q2 >= q1 && q2 >= q3) 81 | { 82 | q0 *= SIGN(eerot[2] - eerot[6]); 83 | q1 *= SIGN(eerot[3] + eerot[1]); 84 | q2 *= +1.0f; 85 | q3 *= SIGN(eerot[7] + eerot[5]); 86 | } 87 | else if (q3 >= q0 && q3 >= q1 && q3 >= q2) 88 | { 89 | q0 *= SIGN(eerot[3] - eerot[1]); 90 | q1 *= SIGN(eerot[6] + eerot[2]); 91 | q2 *= SIGN(eerot[7] + eerot[5]); 92 | q3 *= +1.0f; 93 | } 94 | else 95 | { 96 | printf("Error while converting to quaternion! \n"); 97 | } 98 | float r = NORM(q0, q1, q2, q3); 99 | q0 /= r; 100 | q1 /= r; 101 | q2 /= r; 102 | q3 /= r; 103 | pose.push_back(eetrans[0]); 104 | pose.push_back(eetrans[1]); 105 | pose.push_back(eetrans[2]); 106 | pose.push_back(q1); 107 | pose.push_back(q2); 108 | pose.push_back(q3); 109 | pose.push_back(q0); 110 | } 111 | 112 | bool Kinematics::isIKSuccess(const std::vector< double >& pose, std::vector< double >& joints, int& numOfSolns) 113 | { 114 | #if IK_VERSION > 54 115 | // for IKFast 56,61 116 | unsigned int num_of_joints = GetNumJoints(); 117 | unsigned int num_free_parameters = GetNumFreeParameters(); 118 | #else 119 | // for IKFast 54 120 | unsigned int num_of_joints = getNumJoints(); 121 | unsigned int num_free_parameters = getNumFreeParameters(); 122 | #endif 123 | 124 | #if IK_VERSION > 54 125 | // IKREAL_TYPE joints[num_of_joints]; 126 | // for IKFast 56,61 127 | IkSolutionList< IKREAL_TYPE > solutions; 128 | #else 129 | // for IKFast 54 130 | std::vector< IKSolution > vsolutions; 131 | #endif 132 | std::vector< IKREAL_TYPE > vfree(num_free_parameters); 133 | eetrans[0] = pose[0]; 134 | eetrans[1] = pose[1]; 135 | eetrans[2] = pose[2]; 136 | double qw = pose[6]; 137 | double qx = pose[3]; 138 | double qy = pose[4]; 139 | double qz = pose[5]; 140 | const double n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); 141 | qw *= n; 142 | qx *= n; 143 | qy *= n; 144 | qz *= n; 145 | eerot[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz; 146 | eerot[1] = 2.0f * qx * qy - 2.0f * qz * qw; 147 | eerot[2] = 2.0f * qx * qz + 2.0f * qy * qw; 148 | eerot[3] = 2.0f * qx * qy + 2.0f * qz * qw; 149 | eerot[4] = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz; 150 | eerot[5] = 2.0f * qy * qz - 2.0f * qx * qw; 151 | eerot[6] = 2.0f * qx * qz - 2.0f * qy * qw; 152 | eerot[7] = 2.0f * qy * qz + 2.0f * qx * qw; 153 | eerot[8] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy; 154 | // for(std::size_t i = 0; i < vfree.size(); ++i) 155 | // vfree[i] = atof(argv[13+i]); 156 | // TODO: the user have to define the number of free parameters for the manipulator if it has more than 6 joints. So 157 | // currently more than 6 joints are not supported yet. 158 | 159 | #if IK_VERSION > 54 160 | // for IKFast 56,61 161 | bool b1Success = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); 162 | 163 | #else 164 | // for IKFast 54 165 | bool b2Success = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); 166 | 167 | #endif 168 | #if IK_VERSION > 54 169 | // for IKFast 56,61 170 | unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); 171 | numOfSolns = num_of_solutions; 172 | #else 173 | // for IKFast 54 174 | unsigned int num_of_solutions = (int)vsolutions.size(); 175 | numOfSolns = num_of_solutions; 176 | #endif 177 | 178 | joints.resize(num_of_joints); 179 | 180 | #if IK_VERSION > 54 181 | // for IKFast 56,61 182 | if (!b1Success) 183 | { 184 | return false; 185 | } 186 | else 187 | { 188 | // cout<<"Found ik solutions: "<< num_of_solutions<& sol = solutions.GetSolution(0); 190 | int this_sol_free_params = (int)sol.GetFree().size(); 191 | if( this_sol_free_params <= 0){ 192 | sol.GetSolution(&joints[0], NULL); 193 | } 194 | else{ 195 | static std::vector< IKREAL_TYPE > vsolfree; 196 | vsolfree.resize(this_sol_free_params); 197 | sol.GetSolution(&joints[0], &vsolfree[0]); 198 | } 199 | return true; 200 | } 201 | #else 202 | if (!b2success) 203 | { 204 | return false; 205 | } 206 | else 207 | { 208 | // cout<<"Found ik solutions: "<< num_of_solutions< vsolfree(this_sol_free_params); 211 | vsolutions[i].GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL); 212 | return true; 213 | } 214 | #endif 215 | } 216 | 217 | const std::string Kinematics::getRobotName() 218 | { 219 | const char* hash = GetKinematicsHash(); 220 | 221 | std::string part = hash; 222 | part.erase(0, 22); 223 | std::string name = part.substr(0, part.find(" ")); 224 | return name; 225 | } 226 | 227 | bool Kinematics::isIkSuccesswithTransformedBase(const geometry_msgs::Pose& base_pose, 228 | const geometry_msgs::Pose& grasp_pose, std::vector& joint_soln,int& numOfSolns) 229 | { 230 | // Creating a transformation out of base pose 231 | tf2::Vector3 base_vec(base_pose.position.x, base_pose.position.y, base_pose.position.z); 232 | tf2::Quaternion base_quat(base_pose.orientation.x, base_pose.orientation.y, base_pose.orientation.z, 233 | base_pose.orientation.w); 234 | base_quat.normalize(); 235 | tf2::Transform base_trns; 236 | base_trns.setOrigin(base_vec); 237 | base_trns.setRotation(base_quat); 238 | 239 | // Inverse of the transformation 240 | tf2::Transform base_trns_inv; 241 | base_trns_inv = base_trns.inverse(); 242 | 243 | // Creating a transformation of grasp pose 244 | tf2::Vector3 grasp_vec(grasp_pose.position.x, grasp_pose.position.y, grasp_pose.position.z); 245 | tf2::Quaternion grasp_quat(grasp_pose.orientation.x, grasp_pose.orientation.y, grasp_pose.orientation.z, 246 | grasp_pose.orientation.w); 247 | grasp_quat.normalize(); 248 | tf2::Transform grasp_trns; 249 | grasp_trns.setOrigin(grasp_vec); 250 | grasp_trns.setRotation(grasp_quat); 251 | 252 | // Transforming grasp pose to origin from where we can check for Ik 253 | tf2::Transform new_grasp_trns; 254 | // new_grasp_trns = grasp_trns * base_trns_inv; 255 | new_grasp_trns = base_trns_inv * grasp_trns; 256 | // Creating a new grasp pose in the origin co-ordinate 257 | std::vector< double > new_grasp_pos; 258 | tf2::Vector3 new_grasp_vec; 259 | tf2::Quaternion new_grasp_quat; 260 | new_grasp_vec = new_grasp_trns.getOrigin(); 261 | new_grasp_quat = new_grasp_trns.getRotation(); 262 | new_grasp_quat.normalize(); 263 | new_grasp_pos.push_back(new_grasp_vec[0]); 264 | new_grasp_pos.push_back(new_grasp_vec[1]); 265 | new_grasp_pos.push_back(new_grasp_vec[2]); 266 | new_grasp_pos.push_back(new_grasp_quat[0]); 267 | new_grasp_pos.push_back(new_grasp_quat[1]); 268 | new_grasp_pos.push_back(new_grasp_quat[2]); 269 | new_grasp_pos.push_back(new_grasp_quat[3]); 270 | 271 | // Check the new grasp_pose for Ik 272 | Kinematics k; 273 | //std::vector< double > joints; 274 | 275 | //joints.resize(6); 276 | if (k.isIKSuccess(new_grasp_pos, joint_soln, numOfSolns)) 277 | return true; 278 | else 279 | return false; 280 | } 281 | 282 | 283 | 284 | 285 | 286 | }; 287 | -------------------------------------------------------------------------------- /map_creator/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /base_placement_plugin/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /workspace_visualization/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /map_creator/src/create_capability_map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | //struct stat st; 17 | 18 | bool isFloat(std::string s) 19 | { 20 | std::istringstream iss(s); 21 | float dummy; 22 | iss >> std::noskipws >> dummy; 23 | return iss && iss.eof(); // Result converted to bool 24 | } 25 | 26 | int main(int argc, char **argv) 27 | { 28 | ros::init(argc, argv, "capability_map"); 29 | ros::NodeHandle n; 30 | time_t startit, finish; 31 | time(&startit); 32 | float resolution = 0.08; 33 | kinematics::Kinematics k; 34 | std::string ext = ".h5"; 35 | std::string filename = 36 | str(boost::format("%s_r%d_capability.h5") % k.getRobotName() % resolution); 37 | if (argc == 2) 38 | { 39 | if (!isFloat(argv[1])) 40 | { 41 | ROS_ERROR_STREAM("Probably you have just provided only the map filename. Hey!! The first argument is the " 42 | "resolution."); 43 | return 0; 44 | } 45 | resolution = atof(argv[1]); 46 | filename = 47 | str(boost::format("%s_r%d_capability.h5") % k.getRobotName() % resolution); 48 | } 49 | 50 | else if (argc == 3) 51 | { 52 | std::string name; 53 | name = argv[2]; 54 | if (!isFloat(argv[1]) && isFloat(argv[2])) 55 | { 56 | ROS_ERROR_STREAM("Hey!! The first argument is the resolution and the second argument is the map filename. You " 57 | "messed up."); 58 | return 0; 59 | } 60 | 61 | else if (name.find(ext) == std::string::npos) 62 | { 63 | ROS_ERROR_STREAM("Please provide an extension of .h5 It will make life easy"); 64 | return 0; 65 | } 66 | else 67 | { 68 | resolution = atof(argv[1]); 69 | filename = argv[2]; 70 | } 71 | } 72 | else if (argc < 2) 73 | { 74 | ROS_INFO("You have not provided any argument. So taking default values."); 75 | } 76 | 77 | ros::Rate loop_rate(10); 78 | 79 | int count = 0; 80 | while (ros::ok()) 81 | { 82 | float HI = -1.5, LO = 1.5; 83 | unsigned char max_depth = 16; 84 | unsigned char minDepth = 0; 85 | // A box of radius 1 is created. It will be the size of the robot+1.5. Then the box is discretized by voxels of 86 | // specified resolution 87 | // TODO resolution will be user argument 88 | // The center of every voxels are stored in a vector 89 | 90 | sphere_discretization::SphereDiscretization sd; 91 | float r = 1; 92 | 93 | octomap::point3d origin = octomap::point3d(0, 0, 0); // This point will be the base of the robot 94 | octomap::OcTree *tree = sd.generateBoxTree(origin, r, resolution); 95 | std::vector< octomap::point3d > new_data; 96 | ROS_INFO("Creating the box and discretizing with resolution: %f", resolution); 97 | for (octomap::OcTree::leaf_iterator it = tree->begin_leafs(max_depth), end = tree->end_leafs(); it != end; ++it) 98 | { 99 | new_data.push_back(it.getCoordinate()); 100 | } 101 | ROS_INFO("Total no of spheres now: %lu", new_data.size()); 102 | ROS_INFO("Please hold ON. Spheres are discretized and all of the poses are checked for Ik solutions. May take some " 103 | "time"); 104 | 105 | float radius = resolution; 106 | 107 | std::multimap< std::vector< double >, std::vector< double > > pose_col; 108 | { 109 | std::vector< geometry_msgs::Pose > pose; 110 | std::vector< double > sphere_coord; 111 | std::vector< double > point_on_sphere; 112 | for (int i = 0; i < new_data.size(); i++) 113 | { 114 | sd.convertPointToVector(new_data[i], sphere_coord); 115 | 116 | sd.make_sphere_poses(new_data[i], radius, pose); 117 | for (int j = 0; j < pose.size(); j++) 118 | { 119 | sd.convertPoseToVector(pose[j], point_on_sphere); 120 | pose_col.insert(std::make_pair(point_on_sphere, sphere_coord)); 121 | } 122 | } 123 | } 124 | 125 | // Every pose is checked for IK solutions. The reachable poses and the their corresponsing joint solutions are 126 | // stored. Only the First joint solution is stored. We may need this solutions in the future. Otherwise we can show 127 | // the robot dancing with the joint solutions in a parallel thread 128 | // TODO Support for more than 6DOF robots needs to be implemented. 129 | 130 | kinematics::Kinematics k; 131 | std::multimap< std::vector< double >, std::vector< double > > pose_col_filter; 132 | std::multimap< std::vector< double >, std::vector< double > > pose_col2; 133 | std::vector< std::vector< double > > ik_solutions; 134 | for (std::multimap< std::vector< double >, std::vector< double > >::iterator it = pose_col.begin(); it != pose_col.end(); ++it) 135 | { 136 | std::vector< double > joints; 137 | joints.resize(6); 138 | pose_col2.insert(std::pair< std::vector< double >, std::vector< double > >(it->second, it->first)); 139 | int solns; 140 | if (k.isIKSuccess(it->first, joints, solns)) 141 | { 142 | pose_col_filter.insert(std::pair< std::vector< double >, std::vector< double > >(it->second, it->first)); 143 | ik_solutions.push_back(joints); 144 | } 145 | } 146 | 147 | ROS_INFO("Total number of poses: %lu", pose_col.size()); 148 | ROS_INFO("Total number of reachable poses: %lu", pose_col_filter.size()); 149 | 150 | // The centers of reachable spheres are stored in a map. This data will be utilized in visualizing the spheres in 151 | // the visualizer. 152 | // TODO there are several maps are implemented. We can get rid of few maps and run large loops. The complexity of 153 | // accessing map is Olog(n) 154 | std::vector< std::vector< double > > capability_data; 155 | std::map< std::vector< double >, double > sphere_color; 156 | std::vector< std::vector< double > > poseReach; 157 | for (std::multimap< std::vector< double >, std::vector< double > >::iterator it = pose_col_filter.begin(); it != pose_col_filter.end(); 158 | ++it) 159 | { 160 | // Reachability Index D=R/N*100; 161 | 162 | float d = float(pose_col_filter.count(it->first)) / (pose_col.size() / new_data.size()) * 100; 163 | sphere_color.insert(std::pair< std::vector< double >, double >(it->first, double(d))); 164 | poseReach.push_back(it->second); 165 | } 166 | 167 | ROS_INFO("No of spheres reachable: %lu", sphere_color.size()); 168 | 169 | // Starting capability map 170 | 171 | ROS_INFO("All the outer spheres are checked for optimal pose and optimal openning angles for cone representation. " 172 | "May take some time."); 173 | int i = 0; 174 | for (std::map< std::vector< double >, double >::iterator it = sphere_color.begin(); it != sphere_color.end(); 175 | ++it) // for all the spheres in workspace 176 | { 177 | i += 1; 178 | ROS_INFO ("Processing sphere: %d", i); 179 | 180 | if (it->second <= 20) // All the spheres that have reachability less or equal to 20 181 | { 182 | octomap::point3d sphere_center; 183 | sd.convertVectorToPoint(it->first, sphere_center); // center of sphere 184 | 185 | std::vector< geometry_msgs::Pose > reach_pose_of_sphere; 186 | std::vector< octomap::point3d > reach_points; 187 | 188 | std::multimap< std::vector< double >, std::vector< double > >::iterator it1; // Looking for poses of those spheres 189 | for (it1 = pose_col_filter.lower_bound(it->first); it1 != pose_col_filter.upper_bound(it->first); ++it1) 190 | { 191 | geometry_msgs::Pose pp; 192 | sd.convertVectorToPose(it1->second, pp); // found poses for spheres 193 | 194 | octomap::point3d pose_point; 195 | sd.poseToPoint(pp, pose_point); // only positions are taken from thoses poses 196 | 197 | reach_pose_of_sphere.push_back(pp); // poses in a vector 198 | reach_points.push_back(pose_point); // filtered positions in a vector 199 | } 200 | geometry_msgs::Pose opti_pose; 201 | 202 | // finding optimal pose of the sphere 203 | geometry_msgs::Pose optimal_pose_pca; 204 | sd.findOptimalPosebyPCA(reach_pose_of_sphere, optimal_pose_pca); 205 | 206 | opti_pose.position.x = sphere_center.x(); 207 | opti_pose.position.y = sphere_center.y(); 208 | opti_pose.position.z = sphere_center.z(); 209 | opti_pose.orientation.x = optimal_pose_pca.orientation.x; 210 | opti_pose.orientation.y = optimal_pose_pca.orientation.y; 211 | opti_pose.orientation.z = optimal_pose_pca.orientation.z; 212 | opti_pose.orientation.w = optimal_pose_pca.orientation.w; 213 | 214 | double sfe = 0.0; 215 | std::map< double, double > angle_sfe; 216 | for (double angle = 2; angle <= 10.0; angle += 0.5) 217 | { 218 | pcl::PointCloud< pcl::PointXYZ >::Ptr cloud(new pcl::PointCloud< pcl::PointXYZ >); 219 | sd.createConeCloud(opti_pose, angle, 0.5, cloud); 220 | //cout << "cloud size: " << cloud->size() << endl; 221 | //ROS_INFO_STREAM("cloud size: " << cloud->size()); 222 | double r_poses = 0.0; // Pose that are reachable but not in cone 223 | for (int j = 0; j < reach_points.size(); j++) 224 | { 225 | if (!sd.isPointInCloud(cloud, reach_points[j])) 226 | r_poses += 1; 227 | } 228 | 229 | double R_poses = reach_points.size(); // Total number of filtered pose in that sphere 230 | 231 | std::multimap< std::vector< double >, std::vector< double > >::iterator it2; 232 | std::vector< octomap::point3d > reach_points_sphere; 233 | for (it2 = pose_col2.lower_bound(it->first); it2 != pose_col2.upper_bound(it->first); ++it2) 234 | { 235 | geometry_msgs::Pose pp; 236 | octomap::point3d pose_point; 237 | sd.convertVectorToPose(it2->second, pp); 238 | sd.poseToPoint(pp, pose_point); 239 | reach_points_sphere.push_back(pose_point); 240 | } 241 | double v_poses = 0.0; // poses that are in the cone but not reachable 242 | for (int k = 0; k < reach_points_sphere.size(); k++) 243 | { 244 | if (sd.isPointInCloud(cloud, reach_points_sphere[k])) 245 | if (std::count(reach_points.begin(), reach_points.end(), reach_points_sphere[k]) == 0) 246 | v_poses += 1; 247 | } 248 | sfe = (r_poses + v_poses) / R_poses; 249 | angle_sfe.insert(std::pair< double, double >(sfe, angle)); 250 | } 251 | std::vector< double > capability; 252 | capability.push_back(1.0); // Enum for cone 253 | capability.push_back(it->second); // Reachability index 254 | capability.push_back(angle_sfe.begin()->second); // Optimal cone angle 255 | capability.push_back(opti_pose.position.x); // Position x,y,z 256 | capability.push_back(opti_pose.position.y); 257 | capability.push_back(opti_pose.position.z); 258 | capability.push_back(opti_pose.orientation.x); // Orientation x,y,z,w 259 | capability.push_back(opti_pose.orientation.y); 260 | capability.push_back(opti_pose.orientation.z); 261 | capability.push_back(opti_pose.orientation.w); 262 | capability_data.push_back(capability); 263 | } 264 | 265 | else 266 | { 267 | std::vector< double > capability_sp; 268 | capability_sp.push_back(2.0); 269 | capability_sp.push_back(it->second); 270 | capability_sp.push_back(0.0); 271 | capability_sp.push_back(it->first[0]); 272 | capability_sp.push_back(it->first[1]); 273 | capability_sp.push_back(it->first[2]); 274 | capability_sp.push_back(0.0); 275 | capability_sp.push_back(0.0); 276 | capability_sp.push_back(0.0); 277 | capability_sp.push_back(1.0); 278 | capability_data.push_back(capability_sp); 279 | } 280 | } 281 | ROS_INFO("Capability map is created, saving data to database."); 282 | 283 | // Saving to database 284 | hdf5_dataset::Hdf5Dataset h5(filename); 285 | h5.saveCapMapsToDataset(capability_data, resolution); 286 | 287 | 288 | time(&finish); 289 | double dif = difftime(finish, startit); 290 | ROS_INFO("Elasped time is %.2lf seconds.", dif); 291 | ROS_INFO("Completed"); 292 | ros::spinOnce(); 293 | // sleep(20000); 294 | return 1; 295 | loop_rate.sleep(); 296 | count; 297 | } 298 | 299 | return 0; 300 | } 301 | --------------------------------------------------------------------------------