├── LICENSE ├── README.md ├── changelog ├── semantic_map_common ├── CMakeLists.txt ├── include │ └── semantic_map_common │ │ ├── Action.h │ │ ├── ActionOnObject.h │ │ ├── Address.h │ │ ├── Address.tpp │ │ ├── DataProperty.h │ │ ├── DataProperty.tpp │ │ ├── DataPropertyTraits.h │ │ ├── Entity.h │ │ ├── Entity.tpp │ │ ├── Exceptions.h │ │ ├── Map.h │ │ ├── Mission.h │ │ ├── NamespacePrefix.h │ │ ├── Object.h │ │ ├── ObjectProperty.h │ │ ├── Ontology.h │ │ ├── Pose.h │ │ ├── Property.h │ │ ├── Property.tpp │ │ ├── Size.h │ │ └── Task.h ├── package.xml └── src │ ├── Action.cpp │ ├── ActionOnObject.cpp │ ├── Address.cpp │ ├── DataProperty.cpp │ ├── Entity.cpp │ ├── Exceptions.cpp │ ├── Map.cpp │ ├── Mission.cpp │ ├── NamespacePrefix.cpp │ ├── Object.cpp │ ├── ObjectProperty.cpp │ ├── Ontology.cpp │ ├── Pose.cpp │ ├── Property.cpp │ ├── Size.cpp │ └── Task.cpp ├── semantic_map_conversions ├── CMakeLists.txt ├── include │ └── semantic_map_conversions │ │ ├── MessageConversions.h │ │ ├── MessageConversions.tpp │ │ ├── XmlRpcValueConversions.h │ │ └── XmlRpcValueConversions.tpp ├── package.xml └── src │ ├── MessageConversions.cpp │ └── XmlRpcValueConversions.cpp ├── semantic_map_msgs ├── CMakeLists.txt ├── msg │ ├── ActionFeedback.msg │ ├── ActionOnObject.msg │ ├── Address.msg │ ├── DataProperty.msg │ ├── Map.msg │ ├── Mission.msg │ ├── NamespacePrefix.msg │ ├── Object.msg │ ├── ObjectProperty.msg │ ├── Ontology.msg │ ├── Size.msg │ └── Task.msg ├── package.xml └── srv │ ├── GetMap.srv │ └── GetOntology.srv ├── semantic_map_server ├── CMakeLists.txt ├── config │ └── server.yaml ├── include │ └── semantic_map_server │ │ └── Server.h ├── launch │ ├── server_node.launch │ └── server_nodelet.launch ├── maps │ └── example.yaml ├── nodelet_plugins.xml ├── package.xml └── src │ ├── Server.cpp │ └── server_node.cpp ├── semantic_map_test ├── CMakeLists.txt ├── package.xml └── test │ ├── CommonTest.cpp │ ├── ConversionsTest.cpp │ └── semantic_map_tests.cpp └── semantic_map_to_owl ├── CMakeLists.txt ├── config └── map_to_owl.yaml ├── include └── semantic_map_to_owl │ └── MapToOwl.h ├── launch ├── map_to_owl_node.launch └── map_to_owl_nodelet.launch ├── nodelet_plugins.xml ├── package.xml └── src ├── MapToOwl.cpp └── map_to_owl_node.cpp /README.md: -------------------------------------------------------------------------------- 1 | # Plugins Related to ROS TF Frames 2 | 3 | ## Overview 4 | 5 | **Author(s): Ralf Kaestner
6 | Affiliation: Autonomous Systems Lab, ETH Zurich** 7 | 8 | ## Content 9 | 10 | This project contains the follwing GUI plugins which may be useful when 11 | working with ROS TF frames: 12 | 13 | ### Static TF Plugin for rqt 14 | 15 | ![Static TF Plugin for rqt](rqt_static_tf.png) 16 | 17 | ### TF Marker for rviz 18 | 19 | ![TF Marker for rviz](rviz_tf_marker.png) 20 | 21 | ## Installation 22 | 23 | ### Dependencies 24 | 25 | - [tf2](http://wiki.ros.org/tf2) 26 | 27 | ``` 28 | sudo apt-get install ros-indigo-tf2-ros ros-indigo-tf2-geometry-msgs 29 | ``` 30 | 31 | - [rqt](http://wiki.ros.org/rqt) 32 | 33 | ``` 34 | sudo apt-get install ros-indigo-rqt 35 | ``` 36 | 37 | - [rviz](http://wiki.ros.org/rviz) 38 | 39 | ``` 40 | sudo apt-get install ros-indigo-rviz 41 | ``` 42 | 43 | ### Building 44 | 45 | Create a symlink in your catkin source folder, e.g.: 46 | 47 | ``` 48 | ln -s ~/git/ros-tf-plugins ~/catkin_ws/src 49 | ``` 50 | 51 | If you just need certain componenets of the GUI: 52 | 53 | ``` 54 | ln -s ~/git/ros-tf-plugins/name_of_the_component ~/catkin_ws/src 55 | ``` 56 | 57 | ## Usage 58 | 59 | ### Static TF Plugin for rqt 60 | 61 | To launch the standalone rqt plugin, run 62 | 63 | ``` 64 | rosrun rqt_static_tf rqt_static_tf 65 | ``` 66 | 67 | To launch the rqt GUI without a perspective, run 68 | 69 | ``` 70 | rqt --force-discover 71 | ``` 72 | 73 | This will discover all plugins, which can then be loaded manually. 74 | 75 | To delete the default configuration files (in case of problems): 76 | 77 | ``` 78 | rqt --clear-config 79 | ``` 80 | 81 | ### TF Marker for rviz 82 | 83 | To launch rviz and discover the plugin, run 84 | 85 | ``` 86 | rosrun rviz rviz 87 | ``` 88 | 89 | ## Bugs & Feature Requests 90 | 91 | Please report bugs and feature requests on the 92 | [Issue Tracker](https://github.com/ethz-asl/ros-tf-plugins). 93 | -------------------------------------------------------------------------------- /changelog: -------------------------------------------------------------------------------- 1 | ros-semantic-map (0.0.1-3) unstable; urgency=low 2 | 3 | * Progress commit 4 | 5 | -- Ralf Kaestner Tue, 09 Feb 2016 15:48:00 +0100 6 | 7 | ros-semantic-map (0.0.1-2) unstable; urgency=low 8 | 9 | * Progress commit 10 | 11 | -- Ralf Kaestner Fri, 29 Jan 2016 17:02:00 +0100 12 | 13 | ros-semantic-map (0.0.1-1) unstable; urgency=low 14 | 15 | * Initial commit 16 | 17 | -- Ralf Kaestner Mon, 25 Jan 2016 15:05:00 +0100 18 | -------------------------------------------------------------------------------- /semantic_map_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_common) 3 | 4 | set(CMAKE_CXX_FLAGS -std=c++11) 5 | 6 | find_package( 7 | catkin 8 | REQUIRED 9 | roscpp 10 | semantic_map_msgs 11 | ) 12 | 13 | find_package(PkgConfig) 14 | pkg_check_modules(EIGEN3 REQUIRED eigen3) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS 18 | include 19 | ${EIGEN3_INCLUDE_DIRS} 20 | LIBRARIES 21 | semantic_map_common 22 | DEPENDS 23 | roscpp 24 | semantic_map_msgs 25 | ) 26 | 27 | include_directories( 28 | include 29 | ${EIGEN3_INCLUDE_DIRS} 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | add_library( 34 | semantic_map_common 35 | src/Action.cpp 36 | src/ActionOnObject.cpp 37 | src/Address.cpp 38 | src/DataProperty.cpp 39 | src/Entity.cpp 40 | src/Exceptions.cpp 41 | src/Map.cpp 42 | src/Mission.cpp 43 | src/NamespacePrefix.cpp 44 | src/Object.cpp 45 | src/ObjectProperty.cpp 46 | src/Ontology.cpp 47 | src/Pose.cpp 48 | src/Property.cpp 49 | src/Size.cpp 50 | src/Task.cpp 51 | ) 52 | 53 | target_link_libraries( 54 | semantic_map_common 55 | ${catkin_LIBRARIES} 56 | ) 57 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Action.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Action.h 20 | * \brief Header file providing the Action class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_ACTION_H 24 | #define ROS_SEMANTIC_MAP_ACTION_H 25 | 26 | #include 27 | 28 | namespace semantic_map { 29 | class Object; 30 | 31 | /** \brief Semantic map action 32 | */ 33 | class Action : 34 | public Entity { 35 | public: 36 | /** \brief Default constructor 37 | */ 38 | Action(); 39 | 40 | /** \brief Copy constructor 41 | */ 42 | Action(const Action& src); 43 | 44 | /** \brief Copy constructor (overloaded version taking an entity) 45 | */ 46 | Action(const Entity& src); 47 | 48 | /** \brief Destructor 49 | */ 50 | virtual ~Action(); 51 | 52 | /** \brief Set to true to make this semantic map action an 53 | * asserted action 54 | */ 55 | void setAsserted(bool asserted); 56 | 57 | /** \brief True, if this semantic map action is an asserted action 58 | */ 59 | bool isAsserted() const; 60 | 61 | /** \brief True, if this semantic map action is an action on a 62 | * semantic map object 63 | */ 64 | bool isActionOnObject() const; 65 | 66 | /** \brief True, if this semantic map action is a task 67 | */ 68 | bool isTask() const; 69 | 70 | protected: 71 | friend class Entity; 72 | friend class Task; 73 | 74 | /** \brief Semantic map action (implementation) 75 | */ 76 | class Impl : 77 | public Entity::Impl { 78 | public: 79 | Impl(const std::string& identifier, const std::string& type, 80 | bool asserted); 81 | virtual ~Impl(); 82 | 83 | bool asserted_; 84 | }; 85 | }; 86 | }; 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/ActionOnObject.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file ActionOnObject.h 20 | * \brief Header file providing the ActionOnObject class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_ACTION_ON_OBJECT_H 24 | #define ROS_SEMANTIC_MAP_ACTION_ON_OBJECT_H 25 | 26 | #include 27 | #include 28 | 29 | namespace semantic_map { 30 | class Object; 31 | 32 | /** \brief Semantic map action on object 33 | */ 34 | class ActionOnObject : 35 | public Action { 36 | public: 37 | /** \brief Default constructor 38 | */ 39 | ActionOnObject(); 40 | 41 | /** \brief Copy constructor 42 | */ 43 | ActionOnObject(const ActionOnObject& src); 44 | 45 | /** \brief Copy constructor (overloaded version taking an entity) 46 | */ 47 | ActionOnObject(const Entity& src); 48 | 49 | /** \brief Destructor 50 | */ 51 | virtual ~ActionOnObject(); 52 | 53 | /** \brief Retrieve the object acted on by this semantic map action 54 | * on object 55 | */ 56 | Object getObjectActedOn() const; 57 | 58 | protected: 59 | friend class Action; 60 | friend class Object; 61 | 62 | /** \brief Semantic map action (implementation) 63 | */ 64 | class Impl : 65 | public Action::Impl { 66 | public: 67 | Impl(const std::string& identifier, const std::string& type, bool 68 | asserted, const Object& objectActedOn); 69 | virtual ~Impl(); 70 | 71 | const Object objectActedOn_; 72 | }; 73 | }; 74 | }; 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Address.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Address.h 20 | * \brief Header file providing the Address class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_ADDRESS_H 24 | #define ROS_SEMANTIC_MAP_ADDRESS_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace semantic_map { 35 | /** \brief Semantic map address 36 | */ 37 | class Address { 38 | public: 39 | /** \brief Exception thrown in case of a failure to convert a semantic 40 | * map address from/to an XML-RPC value 41 | */ 42 | class XmlRpcConversionFailed : 43 | public ros::Exception { 44 | public: 45 | XmlRpcConversionFailed(const std::string& description); 46 | }; 47 | 48 | /** \brief Default constructor 49 | */ 50 | Address(const std::string& roomNumber = std::string(), const 51 | std::string& floorNumber = std::string(), const std::string& 52 | streetNumber = std::string(), const std::string& streetName = 53 | std::string(), const std::string& cityName = std::string()); 54 | 55 | /** \brief Copy constructor 56 | */ 57 | Address(const Address& src); 58 | 59 | /** \brief Destructor 60 | */ 61 | virtual ~Address(); 62 | 63 | /** \brief Set the room number of this semantic map address 64 | */ 65 | template void setRoomNumber(const T& number); 66 | 67 | /** \brief Retrieve the room number of this semantic map address 68 | */ 69 | template T getRoomNumber() const; 70 | 71 | /** \brief Set the floor number of this semantic map address 72 | */ 73 | template void setFloorNumber(const T& number); 74 | 75 | /** \brief Retrieve the floor number of this semantic map address 76 | */ 77 | template T getFloorNumber() const; 78 | 79 | /** \brief Set the street number of this semantic map address 80 | */ 81 | template void setStreetNumber(const T& number); 82 | 83 | /** \brief Retrieve the street number of this semantic map address 84 | */ 85 | template T getStreetNumber() const; 86 | 87 | /** \brief Set the street name of this semantic map address 88 | */ 89 | void setStreetName(const std::string& name); 90 | 91 | /** \brief Retrieve the street name of this semantic map address 92 | */ 93 | const std::string& getStreetName() const; 94 | 95 | /** \brief Set the city name of this semantic map address 96 | */ 97 | void setCityName(const std::string& name); 98 | 99 | /** \brief Retrieve the city name of this semantic map address 100 | */ 101 | const std::string& getCityName() const; 102 | 103 | /** \brief Convert this semantic map address to an XML-RPC value 104 | */ 105 | XmlRpc::XmlRpcValue toXmlRpcValue() const; 106 | 107 | /** \brief Convert this semantic map address to a message 108 | */ 109 | semantic_map_msgs::Address toMessage() const; 110 | 111 | protected: 112 | friend class Map; 113 | 114 | /** \brief The room number of this semantic map address 115 | */ 116 | std::string roomNumber_; 117 | 118 | /** \brief The floor number of this semantic map address 119 | */ 120 | std::string floorNumber_; 121 | 122 | /** \brief The street number of this semantic map address 123 | */ 124 | std::string streetNumber_; 125 | 126 | /** \brief The street name of this semantic map address 127 | */ 128 | std::string streetName_; 129 | 130 | /** \brief The city name of this semantic map address 131 | */ 132 | std::string cityName_; 133 | 134 | /** \brief Constructor (overloaded version taking an XML-RPC value) 135 | */ 136 | Address(const XmlRpc::XmlRpcValue& value); 137 | 138 | /** \brief Constructor (overloaded version taking a message) 139 | */ 140 | Address(const semantic_map_msgs::Address& message); 141 | }; 142 | }; 143 | 144 | #include 145 | 146 | #endif 147 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Address.tpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Accessors */ 25 | /*****************************************************************************/ 26 | 27 | template void Address::setRoomNumber(const T& number) { 28 | this->roomNumber_ = boost::lexical_cast(number); 29 | } 30 | 31 | template T Address::getRoomNumber() const { 32 | return boost::lexical_cast(this->roomNumber_); 33 | } 34 | 35 | template void Address::setFloorNumber(const T& number) { 36 | this->floorNumber_ = boost::lexical_cast(number); 37 | } 38 | 39 | template T Address::getFloorNumber() const { 40 | return boost::lexical_cast(this->floorNumber_); 41 | } 42 | 43 | template void Address::setStreetNumber(const T& number) { 44 | this->streetNumber_ = boost::lexical_cast(number); 45 | } 46 | 47 | template T Address::getStreetNumber() const { 48 | return boost::lexical_cast(this->streetNumber_); 49 | } 50 | 51 | } 52 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/DataProperty.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file DataProperty.h 20 | * \brief Header file providing the DataProperty class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_DATA_PROPERTY_H 24 | #define ROS_SEMANTIC_MAP_DATA_PROPERTY_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | 36 | namespace semantic_map { 37 | class Map; 38 | 39 | /** \brief Semantic map data property 40 | */ 41 | class DataProperty : 42 | public Property { 43 | public: 44 | /** \brief Definition of the semantic map data property value type 45 | * enumerable 46 | */ 47 | enum ValueType { 48 | Invalid, 49 | String, 50 | Boolean, 51 | Float, 52 | Integer 53 | }; 54 | 55 | /** \brief Exception thrown in case of a failure to convert a semantic 56 | * map data property from/to an XML-RPC value 57 | */ 58 | class XmlRpcConversionFailed : 59 | public ros::Exception { 60 | public: 61 | XmlRpcConversionFailed(const std::string& description); 62 | }; 63 | 64 | /** \brief Default constructor 65 | */ 66 | DataProperty(); 67 | 68 | /** \brief Copy constructor 69 | */ 70 | DataProperty(const DataProperty& src); 71 | 72 | /** \brief Copy constructor (overloaded version taking a property) 73 | */ 74 | DataProperty(const Property& src); 75 | 76 | /** \brief Destructor 77 | */ 78 | virtual ~DataProperty(); 79 | 80 | /** \brief Retrieve the value type of this semantic map data property 81 | */ 82 | ValueType getValueType() const; 83 | 84 | /** \brief Set the value of this semantic map data property 85 | */ 86 | template void setValue(const T& value); 87 | 88 | /** \brief Retrieve the value of this semantic map data property 89 | */ 90 | template T getValue() const; 91 | 92 | /** \brief Clear the value of this semantic map data property 93 | */ 94 | void clearValue(); 95 | 96 | /** \brief Convert this semantic map data property to an XML-RPC value 97 | */ 98 | XmlRpc::XmlRpcValue toXmlRpcValue() const; 99 | 100 | /** \brief Convert this semantic map data property to a message 101 | */ 102 | semantic_map_msgs::DataProperty toMessage() const; 103 | 104 | protected: 105 | friend class Entity; 106 | friend class Property; 107 | 108 | /** \brief Semantic map data property (implementation) 109 | */ 110 | class Impl : 111 | public Property::Impl { 112 | public: 113 | Impl(const std::string& identifier, const Entity& subject, ValueType 114 | valueType = Invalid, const std::string& value = std::string()); 115 | Impl(const XmlRpc::XmlRpcValue& value, const boost:: 116 | unordered_map& entities); 117 | Impl(const semantic_map_msgs::DataProperty& message, const boost:: 118 | unordered_map& entities); 119 | virtual ~Impl(); 120 | 121 | template void setValue(const T& value); 122 | template T getValue() const; 123 | 124 | ValueType valueType_; 125 | std::string value_; 126 | }; 127 | 128 | /** \brief Constructor (overloaded version taking an identifier, a 129 | * subject entity, an a value) 130 | */ 131 | template DataProperty(const std::string& identifier, 132 | const Entity& subject, const T& value); 133 | 134 | /** \brief Constructor (overloaded version taking an XML-RPC value) 135 | */ 136 | DataProperty(const XmlRpc::XmlRpcValue& value, const boost:: 137 | unordered_map& entities); 138 | 139 | /** \brief Constructor (overloaded version taking a message) 140 | */ 141 | DataProperty(const semantic_map_msgs::DataProperty& message, const 142 | boost::unordered_map& entities); 143 | }; 144 | }; 145 | 146 | #include 147 | 148 | #endif 149 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/DataProperty.tpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace semantic_map { 25 | 26 | /*****************************************************************************/ 27 | /* Constructors and Destructor */ 28 | /*****************************************************************************/ 29 | 30 | template DataProperty::DataProperty(const std::string& 31 | identifier, const Entity& subject, const T& value) { 32 | this->impl_.reset(new Impl(identifier, subject, DataPropertyTraits:: 33 | ValueType, boost::lexical_cast(value))); 34 | } 35 | 36 | /*****************************************************************************/ 37 | /* Accessors */ 38 | /*****************************************************************************/ 39 | 40 | template void DataProperty::setValue(const T& value) { 41 | if (this->impl_.get()) 42 | boost::static_pointer_cast(this->impl_)->template 43 | setValue(value); 44 | } 45 | 46 | template T DataProperty::getValue() const { 47 | if (this->impl_.get()) 48 | return boost::static_pointer_cast(this->impl_)->template 49 | getValue(); 50 | else 51 | return T(); 52 | } 53 | 54 | template void DataProperty::Impl::setValue(const T& value) { 55 | this->valueType_ = DataPropertyTraits::ValueType; 56 | this->value_ = boost::lexical_cast(value); 57 | } 58 | 59 | template T DataProperty::Impl::getValue() const { 60 | return boost::lexical_cast(this->value_); 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/DataPropertyTraits.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file DataPropertyTraits.h 20 | * \brief Header file providing the DataPropertyTraits class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_DATA_PROPERTY_TRAITS_H 24 | #define ROS_SEMANTIC_MAP_DATA_PROPERTY_TRAITS_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | namespace semantic_map { 33 | template class DataPropertyTraits; 34 | 35 | template struct DataPropertyTraits >::type> { 37 | static const DataProperty::ValueType ValueType = DataProperty::String; 38 | }; 39 | 40 | template struct DataPropertyTraits >::type> { 42 | static const DataProperty::ValueType ValueType = DataProperty::Boolean; 43 | }; 44 | 45 | template struct DataPropertyTraits::value, 47 | boost::type_traits::ice_not::value>::value> >:: 48 | type> { 49 | static const DataProperty::ValueType ValueType = DataProperty::Integer; 50 | }; 51 | 52 | template struct DataPropertyTraits >::type> { 54 | static const DataProperty::ValueType ValueType = DataProperty::Float; 55 | }; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Entity.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Entity.h 20 | * \brief Header file providing the Entity class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_ENTITY_H 24 | #define ROS_SEMANTIC_MAP_ENTITY_H 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | namespace semantic_map { 33 | class Action; 34 | class DataProperty; 35 | class Map; 36 | class Mission; 37 | class Object; 38 | class ObjectProperty; 39 | class Property; 40 | 41 | /** \brief Semantic map entity 42 | */ 43 | class Entity { 44 | public: 45 | /** \brief Default constructor 46 | */ 47 | Entity(); 48 | 49 | /** \brief Copy constructor 50 | */ 51 | Entity(const Entity& src); 52 | 53 | /** \brief Destructor 54 | */ 55 | virtual ~Entity(); 56 | 57 | /** \brief Retrieve the identifier of this semantic map entity 58 | */ 59 | std::string getIdentifier() const; 60 | 61 | /** \brief Set the type of this semantic map entity 62 | */ 63 | void setType(const std::string& type); 64 | 65 | /** \brief Retrieve the type of this semantic map entity 66 | */ 67 | std::string getType() const; 68 | 69 | /** \brief Retrieve the parent entity of this semantic map entity 70 | */ 71 | Entity getParent() const; 72 | 73 | /** \brief Retrieve the number of properties of this semantic map 74 | * entity 75 | */ 76 | size_t getNumProperties() const; 77 | 78 | /** \brief Retrieve the properties of this semantic map entity 79 | */ 80 | boost::unordered_multimap getProperties() const; 81 | 82 | /** \brief Retrieve a property of this semantic map entity 83 | */ 84 | std::list getProperties(const std::string& identifier) const; 85 | 86 | /** \brief True, if this semantic map entity has a parent 87 | */ 88 | bool hasParent() const; 89 | 90 | /** \brief True, if this semantic map entity is a semantic map 91 | */ 92 | bool isMap() const; 93 | 94 | /** \brief True, if this semantic map entity is a semantic map mission 95 | */ 96 | bool isMission() const; 97 | 98 | /** \brief True, if this semantic map entity is a semantic map object 99 | */ 100 | bool isObject() const; 101 | 102 | /** \brief True, if this semantic map entity is a semantic map action 103 | */ 104 | bool isAction() const; 105 | 106 | /** \brief True, if this semantic map entity is valid 107 | */ 108 | bool isValid() const; 109 | 110 | /** \brief Add a property to this semantic map entity (overloaded 111 | * version for adding a data property) 112 | */ 113 | template DataProperty addProperty(const std::string& 114 | identifier, const T& value); 115 | 116 | /** \brief Add a property to this semantic map entity (overloaded 117 | * version for adding an object property) 118 | */ 119 | ObjectProperty addProperty(const std::string& identifier, const 120 | Entity& object); 121 | 122 | /** \brief Clear the properties of this semantic map entity 123 | */ 124 | void clearProperties(); 125 | 126 | /** \brief Operator for comparing this semantic map entity to another 127 | * entity 128 | */ 129 | bool operator==(const Entity& entity) const; 130 | 131 | protected: 132 | friend class DataProperty; 133 | friend class ObjectProperty; 134 | 135 | /** \brief Semantic map entity (implementation) 136 | */ 137 | class Impl { 138 | public: 139 | Impl(const std::string& identifier, const std::string& type, const 140 | Entity& parent = Entity()); 141 | virtual ~Impl(); 142 | 143 | const std::string identifier_; 144 | std::string type_; 145 | 146 | const boost::shared_ptr parent_; 147 | 148 | boost::unordered_multimap properties_; 149 | }; 150 | 151 | /** \brief The semantic map entity's implementation 152 | */ 153 | boost::shared_ptr impl_; 154 | }; 155 | }; 156 | 157 | #include 158 | 159 | #endif 160 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Entity.tpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Constructors and Destructor */ 25 | /*****************************************************************************/ 26 | 27 | template Entity::Impl::Impl(const M& message, const Entity& 28 | parent) : 29 | identifier_(message.id), 30 | type_(message.type), 31 | parent_(new Entity(parent)) { 32 | BOOST_ASSERT(!message.id.empty()); 33 | BOOST_ASSERT(!message.type.empty()); 34 | } 35 | 36 | /*****************************************************************************/ 37 | /* Methods */ 38 | /*****************************************************************************/ 39 | 40 | template DataProperty Entity::addProperty(const std::string& 41 | identifier, const T& value) { 42 | if (impl_.get()) { 43 | DataProperty property(identifier, *this, value); 44 | 45 | impl_->properties_.insert(std::make_pair(identifier, property)); 46 | 47 | return property; 48 | } 49 | else 50 | return DataProperty(); 51 | } 52 | 53 | } 54 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Exceptions.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Exceptions.h 20 | * \brief Header file providing the Exceptions class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_EXCEPTIONS_H 24 | #define ROS_SEMANTIC_MAP_EXCEPTIONS_H 25 | 26 | #include 27 | 28 | namespace semantic_map { 29 | /** \brief Exception thrown in case of a failure to convert a semantic 30 | * map or one of its members from or to an XML-RPC value 31 | */ 32 | class XmlRpcConversionFailed : 33 | public ros::Exception { 34 | public: 35 | XmlRpcConversionFailed(const std::string& description); 36 | }; 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Map.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Map.h 20 | * \brief Header file providing the Map class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_H 24 | #define ROS_SEMANTIC_MAP_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace semantic_map { 38 | /** \brief Semantic map 39 | */ 40 | class Map : 41 | public Entity { 42 | public: 43 | /** \brief Default constructor 44 | */ 45 | Map(); 46 | 47 | /** \brief Constructor (overloaded version taking an identifier, a type, 48 | * an ontology, a frame identifier, and a timestamp) 49 | */ 50 | Map(const std::string& identifier, const std::string& type, const 51 | std::string& ns = "http://ros.org/semantic_map/map.owl", const 52 | std::string& frame = "/map", const ros::Time& stamp = ros::Time::now()); 53 | 54 | /** \brief Copy constructor 55 | */ 56 | Map(const Map& src); 57 | 58 | /** \brief Copy constructor (overloaded version taking an entity) 59 | */ 60 | Map(const Entity& src); 61 | 62 | /** \brief Destructor 63 | */ 64 | virtual ~Map(); 65 | 66 | /** \brief Set the ontology of this semantic map 67 | */ 68 | void setOntology(const Ontology& ontology); 69 | 70 | /** \brief Retrieve the ontology of this semantic map 71 | */ 72 | Ontology getOntology() const; 73 | 74 | /** \brief Set the frame identifier of this semantic map 75 | */ 76 | void setFrame(const std::string& frame); 77 | 78 | /** \brief Retrieve the frame identifier of this semantic map 79 | */ 80 | std::string getFrame() const; 81 | 82 | /** \brief Set the time stamp of this semantic map 83 | */ 84 | void setStamp(const ros::Time& stamp); 85 | 86 | /** \brief Retrieve the time stamp of this semantic map 87 | */ 88 | ros::Time getStamp() const; 89 | 90 | /** \brief Set the address of this semantic map 91 | */ 92 | void setAddress(const Address& address); 93 | 94 | /** \brief Retrieve the address of this semantic map 95 | */ 96 | Address getAddress() const; 97 | 98 | /** \brief Retrieve the number of all objects of this semantic map 99 | */ 100 | size_t getNumObjects() const; 101 | 102 | /** \brief Retrieve all objects of this semantic map 103 | */ 104 | boost::unordered_map getObjects() const; 105 | 106 | /** \brief Retrieve an object of this semantic map 107 | */ 108 | Object getObject(const std::string& identifier) const; 109 | 110 | /** \brief Retrieve the begin iterator of this semantic map 111 | */ 112 | boost::unordered_map::iterator begin(); 113 | 114 | /** \brief Retrieve the begin const-iterator of this semantic map 115 | */ 116 | boost::unordered_map::const_iterator begin() const; 117 | 118 | /** \brief Retrieve the end iterator of this semantic map 119 | */ 120 | boost::unordered_map::iterator end(); 121 | 122 | /** \brief Retrieve the end const-iterator of this semantic map 123 | */ 124 | boost::unordered_map::const_iterator end() const; 125 | 126 | /** \brief Add an object to this semantic map 127 | */ 128 | Object addObject(const std::string& identifier, const std::string& type); 129 | 130 | /** \brief Clear the objects of this semantic map 131 | */ 132 | void clearObjects(); 133 | 134 | /** \brief Operator for retrieving an object of this semantic map 135 | */ 136 | Object operator[](const std::string& identifier) const; 137 | 138 | /** \brief Operator for comparing this semantic map to another map 139 | */ 140 | bool operator==(const Map& map) const; 141 | 142 | protected: 143 | friend class Entity; 144 | 145 | /** \brief Semantic map (implementation) 146 | */ 147 | class Impl : 148 | public Entity::Impl { 149 | public: 150 | Impl(const std::string& identifier, const std::string& type, const 151 | std::string& ns, const std::string& frame, const ros::Time& stamp); 152 | virtual ~Impl(); 153 | 154 | Ontology ontology_; 155 | 156 | std::string frame_; 157 | ros::Time stamp_; 158 | 159 | Address address_; 160 | 161 | boost::unordered_map objects_; 162 | }; 163 | }; 164 | }; 165 | 166 | #endif 167 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Mission.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Mission.h 20 | * \brief Header file providing the Mission class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_MISSION_H 24 | #define ROS_SEMANTIC_MAP_MISSION_H 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace semantic_map { 36 | /** \brief Semantic map mission 37 | */ 38 | class Mission : 39 | public Entity { 40 | public: 41 | /** \brief Default constructor 42 | */ 43 | Mission(); 44 | 45 | /** \brief Constructor (overloaded version taking an identifier, a 46 | * type, and an ontology) 47 | */ 48 | Mission(const std::string& identifier, const std::string& 49 | type, const std::string& map, const std::string& ns = 50 | "http://ros.org/semantic_map/mission.owl"); 51 | 52 | /** \brief Copy constructor 53 | */ 54 | Mission(const Mission& src); 55 | 56 | /** \brief Copy constructor (overloaded version taking an entity) 57 | */ 58 | Mission(const Entity& src); 59 | 60 | /** \brief Destructor 61 | */ 62 | virtual ~Mission(); 63 | 64 | /** \brief Set the ontology of this semantic map mission 65 | */ 66 | void setOntology(const Ontology& ontology); 67 | 68 | /** \brief Retrieve the ontology of this semantic map mission 69 | */ 70 | Ontology getOntology() const; 71 | 72 | /** \brief Set the map of this semantic map mission 73 | */ 74 | void setMap(const std::string& map); 75 | 76 | /** \brief Retrieve the map of this semantic map mission 77 | */ 78 | std::string getMap() const; 79 | 80 | /** \brief Retrieve the number of tasks of this semantic map mission 81 | */ 82 | size_t getNumTasks() const; 83 | 84 | /** \brief Retrieve the tasks of this semantic map mission 85 | */ 86 | std::list getTasks() const; 87 | 88 | /** \brief Retrieve the begin iterator of this semantic map mission 89 | */ 90 | std::list::iterator begin(); 91 | 92 | /** \brief Retrieve the begin const-iterator of this semantic map mission 93 | */ 94 | std::list::const_iterator begin() const; 95 | 96 | /** \brief Retrieve the end iterator of this semantic map mission 97 | */ 98 | std::list::iterator end(); 99 | 100 | /** \brief Retrieve the end const-iterator of this semantic map mission 101 | */ 102 | std::list::const_iterator end() const; 103 | 104 | /** \brief Add a task to this semantic map mission 105 | */ 106 | Task addTask(const std::string& identifier, const std::string& type, 107 | bool asserted = false, Task::Quantification quantification = Task:: 108 | Intersection, bool unordered = false); 109 | 110 | /** \brief Clear the tasks of this semantic map mission 111 | */ 112 | void clearTasks(); 113 | 114 | protected: 115 | friend class Entity; 116 | 117 | /** \brief Semantic map (implementation) 118 | */ 119 | class Impl : 120 | public Entity::Impl { 121 | public: 122 | Impl(const std::string& identifier, const std::string& type, const 123 | std::string& map, const std::string& ns); 124 | virtual ~Impl(); 125 | 126 | Ontology ontology_; 127 | 128 | std::string map_; 129 | 130 | std::list tasks_; 131 | }; 132 | }; 133 | }; 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/NamespacePrefix.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file NamespacePrefix.h 20 | * \brief Header file providing the NamespacePrefix class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_NAMESPACE_PREFIX_H 24 | #define ROS_SEMANTIC_MAP_NAMESPACE_PREFIX_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | 36 | namespace semantic_map { 37 | class Action; 38 | class Object; 39 | 40 | /** \brief Semantic map namespace prefix 41 | */ 42 | class NamespacePrefix { 43 | public: 44 | /** \brief Exception thrown in case of a failure to convert a semantic 45 | * map namespace prefix from/to an XML-RPC value 46 | */ 47 | class XmlRpcConversionFailed : 48 | public ros::Exception { 49 | public: 50 | XmlRpcConversionFailed(const std::string& description); 51 | }; 52 | 53 | /** \brief Default constructor 54 | */ 55 | NamespacePrefix(); 56 | 57 | /** \brief Constructor (overloaded version taking a prefix and a 58 | * namespace) 59 | */ 60 | NamespacePrefix(const std::string& prefix, const std::string& ns); 61 | 62 | /** \brief Copy constructor 63 | */ 64 | NamespacePrefix(const NamespacePrefix& src); 65 | 66 | /** \brief Destructor 67 | */ 68 | virtual ~NamespacePrefix(); 69 | 70 | /** \brief Retrieve the prefix of this semantic map namespace prefix 71 | */ 72 | std::string getPrefix() const; 73 | 74 | /** \brief Set the namespace of this semantic map namespace prefix 75 | */ 76 | void setNamespace(const std::string& ns); 77 | 78 | /** \brief Retrieve the namespace of this semantic map namespace prefix 79 | */ 80 | std::string getNamespace() const; 81 | 82 | /** \brief True, if this semantic map namespace prefix is valid 83 | */ 84 | bool isValid() const; 85 | 86 | /** \brief Convert a message to this semantic map namespace prefix 87 | */ 88 | void fromMessage(const semantic_map_msgs::NamespacePrefix& message); 89 | 90 | /** \brief Convert this semantic map namespace prefix to a message 91 | */ 92 | semantic_map_msgs::NamespacePrefix toMessage() const; 93 | 94 | /** \brief Convert an XML-RPC value to this semantic map namespace prefix 95 | */ 96 | void fromXmlRpcValue(const XmlRpc::XmlRpcValue& value); 97 | 98 | /** \brief Convert this semantic map namespace prefix to an XML-RPC value 99 | */ 100 | void toXmlRpcValue(XmlRpc::XmlRpcValue& value) const; 101 | 102 | protected: 103 | /** \brief Semantic map namespace prefix (implementation) 104 | */ 105 | class Impl { 106 | public: 107 | Impl(const std::string& prefix, const std::string& ns); 108 | virtual ~Impl(); 109 | 110 | const std::string prefix_; 111 | std::string ns_; 112 | }; 113 | 114 | /** \brief The semantic map namespace prefix's implementation 115 | */ 116 | boost::shared_ptr impl_; 117 | }; 118 | }; 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Object.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Object.h 20 | * \brief Header file providing the Object class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_OBJECT_H 24 | #define ROS_SEMANTIC_MAP_OBJECT_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | namespace semantic_map { 40 | class ActionOnObject; 41 | 42 | /** \brief Semantic map object 43 | */ 44 | class Object : 45 | public Entity { 46 | public: 47 | /** \brief Exception thrown in case of a failure to convert a semantic 48 | * map object from/to an XML-RPC value 49 | */ 50 | class XmlRpcConversionFailed : 51 | public ros::Exception { 52 | public: 53 | XmlRpcConversionFailed(const std::string& description); 54 | }; 55 | 56 | /** \brief Default constructor 57 | */ 58 | Object(); 59 | 60 | /** \brief Copy constructor 61 | */ 62 | Object(const Object& src); 63 | 64 | /** \brief Copy constructor (overloaded version taking an entity) 65 | */ 66 | Object(const Entity& src); 67 | 68 | /** \brief Destructor 69 | */ 70 | virtual ~Object(); 71 | 72 | /** \brief Retrieve the parent object of this semantic map object 73 | */ 74 | Object getParentObject() const; 75 | 76 | /** \brief Set the frame identifier of this semantic map object 77 | */ 78 | void setFrame(const std::string& frame); 79 | 80 | /** \brief Retrieve the frame identifier of this semantic map object 81 | */ 82 | std::string getFrame() const; 83 | 84 | /** \brief Set the time stamp of this semantic map object 85 | */ 86 | void setStamp(const ros::Time& stamp); 87 | 88 | /** \brief Retrieve the time stamp of this semantic map object 89 | */ 90 | ros::Time getStamp() const; 91 | 92 | /** \brief Set the pose of this semantic map object 93 | */ 94 | void setPose(const Pose& pose); 95 | 96 | /** \brief Retrieve the pose of this semantic map object 97 | */ 98 | Pose getPose() const; 99 | 100 | /** \brief Set the size of this semantic map object 101 | */ 102 | void setSize(const Size& size); 103 | 104 | /** \brief Retrieve the size of this semantic map object 105 | */ 106 | Size getSize() const; 107 | 108 | /** \brief Retrieve the number of all parts of this semantic map object 109 | */ 110 | size_t getNumParts() const; 111 | 112 | /** \brief Retrieve all parts of this semantic map object 113 | */ 114 | boost::unordered_map getParts() const; 115 | 116 | /** \brief Retrieve a part of this semantic map object 117 | */ 118 | Object getPart(const std::string& identifier) const; 119 | 120 | /** \brief Retrieve the number of actions of this semantic map object 121 | */ 122 | size_t getNumActions() const; 123 | 124 | /** \brief Retrieve the actions of this semantic map object 125 | */ 126 | boost::unordered_map getActions() const; 127 | 128 | /** \brief Retrieve an action of this semantic map object 129 | */ 130 | ActionOnObject getAction(const std::string& identifier) const; 131 | 132 | /** \brief Retrieve the begin iterator of this semantic map object 133 | */ 134 | boost::unordered_map::iterator begin(); 135 | 136 | /** \brief Retrieve the begin const-iterator of this semantic map 137 | * object 138 | */ 139 | boost::unordered_map::const_iterator begin() const; 140 | 141 | /** \brief Retrieve the end iterator of this semantic map object 142 | */ 143 | boost::unordered_map::iterator end(); 144 | 145 | /** \brief Retrieve the end const-iterator of this semantic map object 146 | */ 147 | boost::unordered_map::const_iterator end() const; 148 | 149 | /** \brief Add a part to this semantic map object 150 | */ 151 | Object addPart(const std::string& identifier, const std::string& type); 152 | 153 | /** \brief Clear the parts of this semantic map object 154 | */ 155 | void clearParts(); 156 | 157 | /** \brief Add an action to this semantic map object 158 | */ 159 | ActionOnObject addAction(const std::string& identifier, const 160 | std::string& type, bool asserted = false); 161 | 162 | /** \brief Clear the actions of this semantic map object 163 | */ 164 | void clearActions(); 165 | 166 | /** \brief Convert this semantic map object to an XML-RPC value 167 | */ 168 | XmlRpc::XmlRpcValue toXmlRpcValue() const; 169 | 170 | /** \brief Convert this semantic map object to a message 171 | */ 172 | semantic_map_msgs::Object toMessage() const; 173 | 174 | /** \brief Operator for retrieving a part of this semantic map object 175 | */ 176 | Object operator[](const std::string& identifier) const; 177 | 178 | protected: 179 | friend class Entity; 180 | friend class Map; 181 | 182 | /** \brief Semantic map action (implementation) 183 | */ 184 | class Impl : 185 | public Entity::Impl { 186 | public: 187 | Impl(const std::string& identifier, const std::string& type, const 188 | Entity& parent); 189 | virtual ~Impl(); 190 | 191 | std::string frame_; 192 | ros::Time stamp_; 193 | 194 | Pose pose_; 195 | Size size_; 196 | 197 | boost::unordered_map parts_; 198 | boost::unordered_map actions_; 199 | }; 200 | 201 | /** \brief Constructor (overloaded version taking an identifier, 202 | * a type, and a parent entity) 203 | */ 204 | Object(const std::string& identifier, const std::string& type, 205 | const Entity& parent); 206 | }; 207 | }; 208 | 209 | #endif 210 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/ObjectProperty.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file ObjectProperty.h 20 | * \brief Header file providing the ObjectProperty class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_OBJECT_PROPERTY_H 24 | #define ROS_SEMANTIC_MAP_OBJECT_PROPERTY_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace semantic_map { 36 | /** \brief Semantic map object property 37 | */ 38 | class ObjectProperty : 39 | public Property { 40 | public: 41 | /** \brief Exception thrown in case of a failure to convert a semantic 42 | * map object property from/to an XML-RPC value 43 | */ 44 | class XmlRpcConversionFailed : 45 | public ros::Exception { 46 | public: 47 | XmlRpcConversionFailed(const std::string& description); 48 | }; 49 | 50 | /** \brief Default constructor 51 | */ 52 | ObjectProperty(); 53 | 54 | /** \brief Copy constructor 55 | */ 56 | ObjectProperty(const ObjectProperty& src); 57 | 58 | /** \brief Copy constructor (overloaded version taking a property) 59 | */ 60 | ObjectProperty(const Property& src); 61 | 62 | /** \brief Destructor 63 | */ 64 | virtual ~ObjectProperty(); 65 | 66 | /** \brief Set the object of this semantic map object property 67 | */ 68 | void setObject(const Entity& object); 69 | 70 | /** \brief Retrieve the subject of this semantic map object property 71 | */ 72 | Entity getObject() const; 73 | 74 | /** \brief Convert this semantic map object property to an XML-RPC value 75 | */ 76 | XmlRpc::XmlRpcValue toXmlRpcValue() const; 77 | 78 | /** \brief Convert this semantic map object property to a message 79 | */ 80 | semantic_map_msgs::ObjectProperty toMessage() const; 81 | 82 | protected: 83 | friend class Entity; 84 | friend class Property; 85 | 86 | /** \brief Semantic map object property (implementation) 87 | */ 88 | class Impl : 89 | public Property::Impl { 90 | public: 91 | Impl(const std::string& identifier, const Entity& subject, const 92 | Entity& object); 93 | Impl(const XmlRpc::XmlRpcValue& value, const boost:: 94 | unordered_map& entities); 95 | Impl(const semantic_map_msgs::ObjectProperty& message, const boost:: 96 | unordered_map& entities); 97 | virtual ~Impl(); 98 | 99 | Entity object_; 100 | }; 101 | 102 | /** \brief Constructor (overloaded version taking an identifier, a 103 | * subject entity, and an object entity) 104 | */ 105 | ObjectProperty(const std::string& identifier, const Entity& subject, 106 | const Entity& object); 107 | 108 | /** \brief Constructor (overloaded version taking an XML-RPC value) 109 | */ 110 | ObjectProperty(const XmlRpc::XmlRpcValue& value, const boost:: 111 | unordered_map& entities); 112 | 113 | /** \brief Constructor (overloaded version taking a message) 114 | */ 115 | ObjectProperty(const semantic_map_msgs::ObjectProperty& message, const 116 | boost::unordered_map& entities); 117 | }; 118 | }; 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Ontology.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Ontology.h 20 | * \brief Header file providing the Ontology class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_ONTOLOGY_H 24 | #define ROS_SEMANTIC_MAP_ONTOLOGY_H 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | namespace semantic_map { 35 | /** \brief Semantic map ontology 36 | */ 37 | class Ontology { 38 | public: 39 | /** \brief Default constructor 40 | */ 41 | Ontology(); 42 | 43 | /** \brief Constructor (overloaded version taking a namespace) 44 | */ 45 | Ontology(const std::string& ns); 46 | 47 | /** \brief Copy constructor 48 | */ 49 | Ontology(const Ontology& src); 50 | 51 | /** \brief Destructor 52 | */ 53 | virtual ~Ontology(); 54 | 55 | /** \brief Set the namespace of this semantic map ontology 56 | */ 57 | void setNamespace(const std::string& ns); 58 | 59 | /** \brief Retrieve the namespace of this semantic map ontology 60 | */ 61 | std::string getNamespace() const; 62 | 63 | /** \brief Retrieve the number of imports of this semantic map ontology 64 | */ 65 | size_t getNumImports() const; 66 | 67 | /** \brief Retrieve the imports of this semantic map ontology 68 | */ 69 | std::list getImports() const; 70 | 71 | /** \brief Retrieve the number of namespace prefixes of this semantic 72 | * map ontology 73 | */ 74 | size_t getNumPrefixes() const; 75 | 76 | /** \brief Retrieve the namespace prefixes of this semantic map ontology 77 | */ 78 | boost::unordered_map getPrefixes() const; 79 | 80 | /** \brief Retrieve a namespace prefix of this semantic map ontology 81 | */ 82 | NamespacePrefix getPrefix(const std::string& prefix) const; 83 | 84 | /** \brief True, if this semantic map ontology is valid 85 | */ 86 | bool isValid() const; 87 | 88 | /** \brief Add an import to this semantic map ontology 89 | */ 90 | void addImport(const std::string& import); 91 | 92 | /** \brief Clear the imports of this semantic map ontology 93 | */ 94 | void clearImports(); 95 | 96 | /** \brief Add a namespace prefix to this semantic map ontology 97 | * (overloaded version taking a namespace prefix) 98 | */ 99 | void addPrefix(const NamespacePrefix& prefix); 100 | 101 | /** \brief Add a namespace prefix to this semantic map ontology 102 | * (overloaded version taking a prefix and a namespace) 103 | */ 104 | NamespacePrefix addPrefix(const std::string& prefix, const 105 | std::string& ns); 106 | 107 | /** \brief Clear the namespace prefixes of this semantic map ontology 108 | */ 109 | void clearPrefixes(); 110 | 111 | protected: 112 | /** \brief Semantic map ontology (implementation) 113 | */ 114 | class Impl { 115 | public: 116 | Impl(const std::string& ns); 117 | virtual ~Impl(); 118 | 119 | std::string ns_; 120 | 121 | std::list imports_; 122 | boost::unordered_map prefixes_; 123 | }; 124 | 125 | /** \brief The semantic map ontology's implementation 126 | */ 127 | boost::shared_ptr impl_; 128 | }; 129 | }; 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Pose.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Pose.h 20 | * \brief Header file providing the Pose class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_POSE_H 24 | #define ROS_SEMANTIC_MAP_POSE_H 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | namespace semantic_map { 36 | /** \brief Semantic map size 37 | */ 38 | class Pose { 39 | public: 40 | /** \brief Exception thrown in case of a failure to convert a semantic 41 | * map pose from/to an XML-RPC value 42 | */ 43 | class XmlRpcConversionFailed : 44 | public ros::Exception { 45 | public: 46 | XmlRpcConversionFailed(const std::string& description); 47 | }; 48 | 49 | /** \brief Default constructor 50 | */ 51 | Pose(double x = 0.0, double y = 0.0, double z = 0.0, double i = 0.0, 52 | double j = 0.0, double k = 0.0, double w = 1.0); 53 | 54 | /** \brief Copy constructor 55 | */ 56 | Pose(const Pose& src); 57 | 58 | /** \brief Destructor 59 | */ 60 | virtual ~Pose(); 61 | 62 | /** \brief Set the position of this pose 63 | */ 64 | void setPosition(const Eigen::Vector3d& position); 65 | 66 | /** \brief Retrieve the position of this pose 67 | */ 68 | const Eigen::Vector3d& getPosition() const; 69 | 70 | /** \brief Set the orientation of this pose 71 | */ 72 | void setOrientation(const Eigen::Quaterniond& orientation); 73 | 74 | /** \brief Retrieve the orientation of this pose 75 | */ 76 | const Eigen::Quaterniond& getOrientation() const; 77 | 78 | /** \brief Convert this semantic map pose to an XML-RPC value 79 | */ 80 | XmlRpc::XmlRpcValue toXmlRpcValue() const; 81 | 82 | /** \brief Convert this semantic map pose to a message 83 | */ 84 | geometry_msgs::Pose toMessage() const; 85 | 86 | protected: 87 | friend class Object; 88 | 89 | /** \brief The position of this pose 90 | */ 91 | Eigen::Vector3d position_; 92 | 93 | /** \brief The orientation of this pose 94 | */ 95 | Eigen::Quaterniond orientation_; 96 | 97 | /** \brief Constructor (overloaded version taking an XML-RPC value) 98 | */ 99 | Pose(const XmlRpc::XmlRpcValue& value); 100 | 101 | /** \brief Constructor (overloaded version taking a message) 102 | */ 103 | Pose(const geometry_msgs::Pose& message); 104 | }; 105 | }; 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Property.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Property.h 20 | * \brief Header file providing the Property class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_PROPERTY_H 24 | #define ROS_SEMANTIC_MAP_PROPERTY_H 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | namespace semantic_map { 36 | class DataProperty; 37 | class Entity; 38 | class ObjectProperty; 39 | 40 | /** \brief Semantic map property 41 | */ 42 | class Property { 43 | public: 44 | /** \brief Exception thrown in case of a failure to convert a semantic 45 | * map property from/to an XML-RPC value 46 | */ 47 | class XmlRpcConversionFailed : 48 | public ros::Exception { 49 | public: 50 | XmlRpcConversionFailed(const std::string& description); 51 | }; 52 | 53 | /** \brief Default constructor 54 | */ 55 | Property(); 56 | 57 | /** \brief Copy constructor 58 | */ 59 | Property(const Property& src); 60 | 61 | /** \brief Destructor 62 | */ 63 | virtual ~Property(); 64 | 65 | /** \brief Retrieve the identifier of this semantic map property 66 | */ 67 | std::string getIdentifier() const; 68 | 69 | /** \brief Retrieve the subject of this semantic map property 70 | */ 71 | Entity getSubject() const; 72 | 73 | /** \brief True, if this semantic map property is a data property 74 | */ 75 | bool isDataProperty() const; 76 | 77 | /** \brief True, if this semantic map property is an object property 78 | */ 79 | bool isObjectProperty() const; 80 | 81 | /** \brief True, if this semantic map property is valid 82 | */ 83 | bool isValid() const; 84 | 85 | /** \brief Convert this semantic map property to an XML-RPC value 86 | */ 87 | virtual XmlRpc::XmlRpcValue toXmlRpcValue() const; 88 | 89 | /** \brief Convert this semantic map property to a message 90 | */ 91 | template M toMessage() const; 92 | 93 | protected: 94 | friend class Entity; 95 | 96 | /** \brief Semantic map property (implementation) 97 | */ 98 | class Impl { 99 | public: 100 | Impl(const std::string& identifier, const Entity& subject); 101 | Impl(const XmlRpc::XmlRpcValue& value, const boost::unordered_map< 102 | std::string, Entity>& entities); 103 | template Impl(const M& message, const boost::unordered_map< 104 | std::string, Entity>& entities); 105 | virtual ~Impl(); 106 | 107 | const std::string identifier_; 108 | const boost::shared_ptr subject_; 109 | }; 110 | 111 | /** \brief The semantic map property's implementation 112 | */ 113 | boost::shared_ptr impl_; 114 | }; 115 | }; 116 | 117 | #include 118 | 119 | #endif 120 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Property.tpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | 23 | namespace semantic_map { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | template Property::Impl::Impl(const M& message, const boost:: 30 | unordered_map& entities) : 31 | identifier_(message.id) { 32 | BOOST_ASSERT(!message.id.empty()); 33 | 34 | boost::unordered_map::const_iterator it = entities. 35 | find(message.subject); 36 | 37 | BOOST_ASSERT(it != entities.end()); 38 | BOOST_ASSERT(it->second.isValid()); 39 | 40 | const_cast&>(this->subject_).reset( 41 | new Entity(it->second)); 42 | } 43 | 44 | /*****************************************************************************/ 45 | /* Methods */ 46 | /*****************************************************************************/ 47 | 48 | template M Property::toMessage() const { 49 | M message; 50 | 51 | message.id = this->getIdentifier(); 52 | message.subject = this->getSubject().getIdentifier(); 53 | 54 | return message; 55 | } 56 | 57 | } 58 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Size.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Size.h 20 | * \brief Header file providing the Size class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_SIZE_H 24 | #define ROS_SEMANTIC_MAP_SIZE_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace semantic_map { 35 | /** \brief Semantic map size 36 | */ 37 | class Size { 38 | public: 39 | /** \brief Exception thrown in case of a failure to convert a semantic 40 | * map size from/to an XML-RPC value 41 | */ 42 | class XmlRpcConversionFailed : 43 | public ros::Exception { 44 | public: 45 | XmlRpcConversionFailed(const std::string& description); 46 | }; 47 | 48 | /** \brief Default constructor 49 | */ 50 | Size(double width = 0.0, double height = 0.0, double depth = 0.0); 51 | 52 | /** \brief Copy constructor 53 | */ 54 | Size(const Size& src); 55 | 56 | /** \brief Destructor 57 | */ 58 | virtual ~Size(); 59 | 60 | /** \brief Set the width dimension of this size 61 | */ 62 | void setWidth(double width); 63 | 64 | /** \brief Retrieve the width dimension of this size 65 | */ 66 | double getWidth() const; 67 | 68 | /** \brief Set the height dimension of this size 69 | */ 70 | void setHeight(double height); 71 | 72 | /** \brief Retrieve the height dimension of this size 73 | */ 74 | double getHeight() const; 75 | 76 | /** \brief Set the depth dimension of this size 77 | */ 78 | void setDepth(double depth); 79 | 80 | /** \brief Retrieve the depth dimension of this size 81 | */ 82 | double getDepth() const; 83 | 84 | /** \brief Set the dimensions of this size 85 | */ 86 | void setDimensions(const Eigen::Vector3d& dimensions); 87 | 88 | /** \brief Retrieve the dimensions of this size 89 | */ 90 | const Eigen::Vector3d& getDimensions() const; 91 | 92 | /** \brief Convert this semantic map size to an XML-RPC value 93 | */ 94 | XmlRpc::XmlRpcValue toXmlRpcValue() const; 95 | 96 | /** \brief Convert this semantic map size to a message 97 | */ 98 | semantic_map_msgs::Size toMessage() const; 99 | 100 | protected: 101 | friend class Object; 102 | 103 | /** \brief The dimensions of this size 104 | */ 105 | Eigen::Vector3d dimensions_; 106 | 107 | /** \brief Constructor (overloaded version taking an XML-RPC value) 108 | */ 109 | Size(const XmlRpc::XmlRpcValue& value); 110 | 111 | /** \brief Constructor (overloaded version taking a message) 112 | */ 113 | Size(const semantic_map_msgs::Size& message); 114 | }; 115 | }; 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /semantic_map_common/include/semantic_map_common/Task.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Task.h 20 | * \brief Header file providing the Task class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_TASK_H 24 | #define ROS_SEMANTIC_MAP_TASK_H 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace semantic_map { 32 | /** \brief Semantic map task 33 | */ 34 | class Task : 35 | public Action { 36 | public: 37 | /** \brief Definition of the semantic map task quantification 38 | * enumerable 39 | */ 40 | enum Quantification { 41 | Intersection, 42 | Union 43 | }; 44 | 45 | /** \brief Default constructor 46 | */ 47 | Task(); 48 | 49 | /** \brief Copy constructor 50 | */ 51 | Task(const Task& src); 52 | 53 | /** \brief Copy constructor (overloaded version taking an entity) 54 | */ 55 | Task(const Entity& src); 56 | 57 | /** \brief Destructor 58 | */ 59 | virtual ~Task(); 60 | 61 | /** \brief Set the quantification of this semantic map task 62 | */ 63 | void setQuantification(Quantification quantification); 64 | 65 | /** \brief Retrieve the quantification of this semantic map task 66 | */ 67 | Quantification getQuantification() const; 68 | 69 | /** \brief Retrieve the number of actions of this semantic map task 70 | */ 71 | size_t getNumActions() const; 72 | 73 | /** \brief Retrieve the actions of this semantic map task 74 | */ 75 | std::list getActions() const; 76 | 77 | /** \brief Set to true to flag the actions of this semantic map 78 | * task as unordered 79 | */ 80 | void setUnordered(bool unordered); 81 | 82 | /** \brief True, if the actions of this semantic map task are 83 | * unordered 84 | */ 85 | bool isUnordered() const; 86 | 87 | /** \brief Retrieve the begin iterator of this semantic map task 88 | */ 89 | std::list::iterator begin(); 90 | 91 | /** \brief Retrieve the begin const-iterator of this semantic map task 92 | */ 93 | std::list::const_iterator begin() const; 94 | 95 | /** \brief Retrieve the end iterator of this semantic map task 96 | */ 97 | std::list::iterator end(); 98 | 99 | /** \brief Retrieve the end const-iterator of this semantic map task 100 | */ 101 | std::list::const_iterator end() const; 102 | 103 | /** \brief Add an action to this semantic map task 104 | */ 105 | void addAction(const Action& action); 106 | 107 | /** \brief Add a subtask to this semantic map task 108 | */ 109 | Task addTask(const std::string& identifier, const std::string& type, 110 | bool asserted = false, Quantification quantification = Intersection, 111 | bool unordered = false); 112 | 113 | /** \brief Clear the actions of this semantic map task 114 | */ 115 | void clearActions(); 116 | 117 | protected: 118 | friend class Action; 119 | friend class Mission; 120 | 121 | /** \brief Semantic map task (implementation) 122 | */ 123 | class Impl : 124 | public Action::Impl { 125 | public: 126 | Impl(const std::string& identifier, const std::string& type, 127 | bool asserted, Quantification quantification, bool unordered); 128 | virtual ~Impl(); 129 | 130 | std::list actions_; 131 | Quantification quantification_; 132 | bool unordered_; 133 | }; 134 | }; 135 | }; 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /semantic_map_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_common 3 | 0.0.1 4 | 5 | Backend library for using semantic maps in ROS. 6 | 7 | Ralf Kaestner 8 | 9 | LGPL 10 | 11 | Ralf Kaestner 12 | 13 | catkin 14 | 15 | roscpp 16 | semantic_map_msgs 17 | 18 | roscpp 19 | semantic_map_msgs 20 | 21 | -------------------------------------------------------------------------------- /semantic_map_common/src/Action.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | #include 21 | 22 | #include "semantic_map_common/Action.h" 23 | 24 | namespace semantic_map { 25 | 26 | /*****************************************************************************/ 27 | /* Constructors and Destructor */ 28 | /*****************************************************************************/ 29 | 30 | Action::Action() { 31 | } 32 | 33 | Action::Action(const Action& src) : 34 | Entity(src) { 35 | } 36 | 37 | Action::Action(const Entity& src) : 38 | Entity(src) { 39 | if (impl_.get()) 40 | BOOST_ASSERT(boost::dynamic_pointer_cast(impl_).get()); 41 | } 42 | 43 | Action::~Action() { 44 | } 45 | 46 | Action::Impl::Impl(const std::string& identifier, const std::string& type, 47 | bool asserted) : 48 | Entity::Impl(identifier, type), 49 | asserted_(asserted) { 50 | } 51 | 52 | // Action::Impl::Impl(const XmlRpc::XmlRpcValue& value, const Entity& parent) : 53 | // Entity::Impl(value, parent), 54 | // asserted_(false) { 55 | // try { 56 | // if (value.hasMember("asserted")) 57 | // asserted_ = (bool)const_cast(value)["asserted"]; 58 | // } 59 | // catch (const XmlRpc::XmlRpcException& exception) { 60 | // throw XmlRpcConversionFailed(exception.getMessage()); 61 | // } 62 | // } 63 | 64 | Action::Impl::~Impl() { 65 | } 66 | 67 | /*****************************************************************************/ 68 | /* Accessors */ 69 | /*****************************************************************************/ 70 | 71 | void Action::setAsserted(bool asserted) { 72 | if (impl_.get()) 73 | boost::static_pointer_cast(impl_)->asserted_ = asserted; 74 | } 75 | 76 | bool Action::isAsserted() const { 77 | if (impl_.get()) 78 | return boost::static_pointer_cast(impl_)->asserted_; 79 | else 80 | return false; 81 | } 82 | 83 | bool Action::isActionOnObject() const { 84 | return boost::dynamic_pointer_cast(impl_).get(); 85 | } 86 | 87 | bool Action::isTask() const { 88 | return boost::dynamic_pointer_cast(impl_).get(); 89 | } 90 | 91 | /*****************************************************************************/ 92 | /* Methods */ 93 | /*****************************************************************************/ 94 | 95 | // XmlRpc::XmlRpcValue Action::toXmlRpcValue() const { 96 | // XmlRpc::XmlRpcValue value = Entity::toXmlRpcValue(); 97 | // 98 | // if (impl_.get()) { 99 | // try { 100 | // value["asserted"] = isAsserted(); 101 | // } 102 | // catch (const XmlRpc::XmlRpcException& exception) { 103 | // throw XmlRpcConversionFailed(exception.getMessage()); 104 | // } 105 | // } 106 | // 107 | // return value; 108 | // } 109 | 110 | } 111 | -------------------------------------------------------------------------------- /semantic_map_common/src/ActionOnObject.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_common/ActionOnObject.h" 22 | 23 | namespace semantic_map { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | ActionOnObject::ActionOnObject() { 30 | } 31 | 32 | ActionOnObject::ActionOnObject(const ActionOnObject& src) : 33 | Action(src) { 34 | } 35 | 36 | ActionOnObject::ActionOnObject(const Entity& src) : 37 | Action(src) { 38 | if (impl_.get()) 39 | BOOST_ASSERT(boost::dynamic_pointer_cast(impl_).get()); 40 | } 41 | 42 | ActionOnObject::~ActionOnObject() { 43 | } 44 | 45 | ActionOnObject::Impl::Impl(const std::string& identifier, const std::string& 46 | type, bool asserted, const Object& objectActedOn) : 47 | Action::Impl(identifier, type, asserted), 48 | objectActedOn_(objectActedOn) { 49 | BOOST_ASSERT(objectActedOn.isValid()); 50 | } 51 | 52 | // ActionOnObject::Impl::Impl(const XmlRpc::XmlRpcValue& value, const 53 | // boost::unordered_map& objects) : 54 | // Action::Impl(value) { 55 | // std::string objectActedOn; 56 | // 57 | // try { 58 | // objectActedOn = (std::string)const_cast( 59 | // value)["object_acted_on"]; 60 | // } 61 | // catch (const XmlRpc::XmlRpcException& exception) { 62 | // throw XmlRpcConversionFailed(exception.getMessage()); 63 | // } 64 | // 65 | // boost::unordered_map::const_iterator it = objects. 66 | // find(objectActedOn); 67 | // 68 | // BOOST_ASSERT(it != objects.end()); 69 | // BOOST_ASSERT(it->second.isValid()); 70 | // 71 | // *parent_ = it->second; 72 | // } 73 | 74 | ActionOnObject::Impl::~Impl() { 75 | } 76 | 77 | /*****************************************************************************/ 78 | /* Accessors */ 79 | /*****************************************************************************/ 80 | 81 | Object ActionOnObject::getObjectActedOn() const { 82 | if (impl_.get()) 83 | return boost::static_pointer_cast(impl_)->objectActedOn_; 84 | else 85 | return Object(); 86 | } 87 | 88 | /*****************************************************************************/ 89 | /* Methods */ 90 | /*****************************************************************************/ 91 | 92 | // XmlRpc::XmlRpcValue ActionOnObject::toXmlRpcValue() const { 93 | // XmlRpc::XmlRpcValue value = Action::toXmlRpcValue(); 94 | // 95 | // if (impl_.get()) { 96 | // try { 97 | // value["object_acted_on"] = getObjectActedOn().getIdentifier(); 98 | // } 99 | // catch (const XmlRpc::XmlRpcException& exception) { 100 | // throw XmlRpcConversionFailed(exception.getMessage()); 101 | // } 102 | // } 103 | // 104 | // return value; 105 | // } 106 | 107 | } 108 | -------------------------------------------------------------------------------- /semantic_map_common/src/Address.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include "semantic_map_common/Address.h" 24 | 25 | namespace semantic_map { 26 | 27 | /*****************************************************************************/ 28 | /* Constructors and Destructor */ 29 | /*****************************************************************************/ 30 | 31 | Address::XmlRpcConversionFailed::XmlRpcConversionFailed(const std::string& 32 | description) : 33 | ros::Exception("XML-RPC value conversion of semantic map address failed: "+ 34 | description) { 35 | } 36 | 37 | Address::Address(const std::string& roomNumber, const std::string& 38 | floorNumber, const std::string& streetNumber, const std::string& 39 | streetName, const std::string& cityName) : 40 | roomNumber_(roomNumber), 41 | floorNumber_(floorNumber), 42 | streetNumber_(streetNumber), 43 | streetName_(streetName), 44 | cityName_(cityName) { 45 | } 46 | 47 | Address::Address(const XmlRpc::XmlRpcValue& value) { 48 | try { 49 | if (value.hasMember("room_nr")) { 50 | std::ostringstream stream; 51 | const_cast(value)["room_nr"].write(stream); 52 | 53 | roomNumber_ = stream.str(); 54 | } 55 | 56 | if (value.hasMember("floor_nr")) { 57 | std::ostringstream stream; 58 | const_cast(value)["floor_nr"].write(stream); 59 | 60 | floorNumber_ = stream.str(); 61 | } 62 | 63 | if (value.hasMember("street_nr")) { 64 | std::ostringstream stream; 65 | const_cast(value)["street_nr"].write(stream); 66 | 67 | streetNumber_ = stream.str(); 68 | } 69 | 70 | if (value.hasMember("street_name")) 71 | streetName_ = (std::string)const_cast( 72 | value)["street_name"]; 73 | 74 | if (value.hasMember("city_name")) 75 | cityName_ = (std::string)const_cast( 76 | value)["city_name"]; 77 | } 78 | catch (const XmlRpc::XmlRpcException& exception) { 79 | throw XmlRpcConversionFailed(exception.getMessage()); 80 | } 81 | } 82 | 83 | Address::Address(const semantic_map_msgs::Address& message) : 84 | Address(message.room_nr, message.floor_nr, message.street_nr, 85 | message.street_name, message.city_name) { 86 | } 87 | 88 | Address::Address(const Address& src) : 89 | roomNumber_(src.roomNumber_), 90 | floorNumber_(src.floorNumber_), 91 | streetNumber_(src.streetNumber_), 92 | streetName_(src.streetName_), 93 | cityName_(src.cityName_) { 94 | } 95 | 96 | Address::~Address() { 97 | } 98 | 99 | /*****************************************************************************/ 100 | /* Accessors */ 101 | /*****************************************************************************/ 102 | 103 | void Address::setStreetName(const std::string& name) { 104 | streetName_ = name; 105 | } 106 | 107 | const std::string& Address::getStreetName() const { 108 | return streetName_; 109 | } 110 | 111 | void Address::setCityName(const std::string& name) { 112 | cityName_ = name; 113 | } 114 | 115 | const std::string& Address::getCityName() const { 116 | return cityName_; 117 | } 118 | 119 | /*****************************************************************************/ 120 | /* Methods */ 121 | /*****************************************************************************/ 122 | 123 | XmlRpc::XmlRpcValue Address::toXmlRpcValue() const { 124 | XmlRpc::XmlRpcValue value; 125 | 126 | try { 127 | value["room_nr"] = roomNumber_; 128 | value["floor_nr"] = floorNumber_; 129 | value["street_nr"] = streetNumber_; 130 | value["street_name"] = streetName_; 131 | value["city_name"] = cityName_; 132 | } 133 | catch (const XmlRpc::XmlRpcException& exception) { 134 | throw XmlRpcConversionFailed(exception.getMessage()); 135 | } 136 | 137 | return value; 138 | } 139 | 140 | semantic_map_msgs::Address Address::toMessage() const { 141 | semantic_map_msgs::Address message; 142 | 143 | message.room_nr = roomNumber_; 144 | message.floor_nr = floorNumber_; 145 | message.street_nr = streetNumber_; 146 | message.street_name = streetName_; 147 | message.city_name = cityName_; 148 | 149 | return message; 150 | } 151 | 152 | } 153 | -------------------------------------------------------------------------------- /semantic_map_common/src/DataProperty.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include "semantic_map_common/DataProperty.h" 26 | 27 | namespace semantic_map { 28 | 29 | /*****************************************************************************/ 30 | /* Constructors and Destructor */ 31 | /*****************************************************************************/ 32 | 33 | DataProperty::XmlRpcConversionFailed::XmlRpcConversionFailed(const 34 | std::string& description) : 35 | ros::Exception("XML-RPC value conversion of semantic map data property " 36 | "failed: "+description) { 37 | } 38 | 39 | DataProperty::DataProperty() { 40 | } 41 | 42 | DataProperty::DataProperty(const XmlRpc::XmlRpcValue& value, const 43 | boost::unordered_map& entities) { 44 | impl_.reset(new Impl(value, entities)); 45 | } 46 | 47 | DataProperty::DataProperty(const semantic_map_msgs::DataProperty& message, 48 | const boost::unordered_map& entities) { 49 | impl_.reset(new Impl(message, entities)); 50 | } 51 | 52 | DataProperty::DataProperty(const DataProperty& src) : 53 | Property(src) { 54 | } 55 | 56 | DataProperty::DataProperty(const Property& src) : 57 | Property(src) { 58 | if (impl_.get()) 59 | BOOST_ASSERT(boost::dynamic_pointer_cast(impl_).get()); 60 | } 61 | 62 | DataProperty::~DataProperty() { 63 | } 64 | 65 | DataProperty::Impl::Impl(const std::string& identifier, const Entity& 66 | subject, ValueType valueType, const std::string& value) : 67 | Property::Impl(identifier, subject), 68 | valueType_(valueType), 69 | value_(value) { 70 | BOOST_ASSERT(valueType != Invalid); 71 | } 72 | 73 | DataProperty::Impl::Impl(const XmlRpc::XmlRpcValue& value, const 74 | boost::unordered_map& entities) : 75 | Property::Impl(value, entities) { 76 | try { 77 | if (value.hasMember("value")) { 78 | XmlRpc::XmlRpcValue::Type type = const_cast( 79 | value)["value"].getType(); 80 | 81 | if (type == XmlRpc::XmlRpcValue::TypeString) 82 | setValue(const_cast( 83 | value)["value"]); 84 | else if (type == XmlRpc::XmlRpcValue::TypeBoolean) 85 | setValue(const_cast(value)["value"]); 86 | else if (type == XmlRpc::XmlRpcValue::TypeDouble) 87 | setValue(const_cast(value)["value"]); 88 | else if (type == XmlRpc::XmlRpcValue::TypeInt) 89 | setValue(const_cast(value)["value"]); 90 | } 91 | } 92 | catch (const XmlRpc::XmlRpcException& exception) { 93 | throw XmlRpcConversionFailed(exception.getMessage()); 94 | } 95 | 96 | BOOST_ASSERT(valueType_ != Invalid); 97 | } 98 | 99 | DataProperty::Impl::Impl(const semantic_map_msgs::DataProperty& message, 100 | const boost::unordered_map& entities) : 101 | Property::Impl(message, entities) { 102 | if (message.value_type == semantic_map_msgs::DataProperty:: 103 | VALUE_TYPE_STRING) 104 | setValue(message.value); 105 | else if (message.value_type == semantic_map_msgs::DataProperty:: 106 | VALUE_TYPE_BOOL) 107 | setValue(boost::lexical_cast(message.value)); 108 | else if (message.value_type == semantic_map_msgs::DataProperty:: 109 | VALUE_TYPE_FLOAT) 110 | setValue(boost::lexical_cast(message.value)); 111 | else if (message.value_type == semantic_map_msgs::DataProperty:: 112 | VALUE_TYPE_INT) 113 | setValue(boost::lexical_cast(message.value)); 114 | 115 | BOOST_ASSERT(valueType_ != Invalid); 116 | } 117 | 118 | DataProperty::Impl::~Impl() { 119 | } 120 | 121 | /*****************************************************************************/ 122 | /* Accessors */ 123 | /*****************************************************************************/ 124 | 125 | DataProperty::ValueType DataProperty::getValueType() const { 126 | if (impl_.get()) 127 | return boost::static_pointer_cast(impl_)->valueType_; 128 | else 129 | return Invalid; 130 | } 131 | 132 | /*****************************************************************************/ 133 | /* Methods */ 134 | /*****************************************************************************/ 135 | 136 | void DataProperty::clearValue() { 137 | if (impl_.get()) { 138 | boost::static_pointer_cast(impl_)->valueType_ = Invalid; 139 | boost::static_pointer_cast(impl_)->value_.clear(); 140 | } 141 | } 142 | 143 | XmlRpc::XmlRpcValue DataProperty::toXmlRpcValue() const { 144 | XmlRpc::XmlRpcValue value = Property::toXmlRpcValue(); 145 | 146 | if (impl_.get()) { 147 | try { 148 | ValueType valueType = getValueType(); 149 | 150 | if (valueType == String) 151 | value["value"] = getValue(); 152 | else if (valueType == Boolean) 153 | value["value"] = getValue(); 154 | else if (valueType == Float) 155 | value["value"] = getValue(); 156 | else if (valueType == Integer) 157 | value["value"] = getValue(); 158 | else 159 | value["value"] = XmlRpc::XmlRpcValue(); 160 | } 161 | catch (const XmlRpc::XmlRpcException& exception) { 162 | throw XmlRpcConversionFailed(exception.getMessage()); 163 | } 164 | } 165 | 166 | return value; 167 | } 168 | 169 | semantic_map_msgs::DataProperty DataProperty::toMessage() const { 170 | semantic_map_msgs::DataProperty message = Property:: 171 | toMessage(); 172 | 173 | ValueType valueType = getValueType(); 174 | 175 | if (valueType == String) 176 | message.value_type = semantic_map_msgs::DataProperty::VALUE_TYPE_STRING; 177 | else if (valueType == Boolean) 178 | message.value_type = semantic_map_msgs::DataProperty::VALUE_TYPE_BOOL; 179 | else if (valueType == Float) 180 | message.value_type = semantic_map_msgs::DataProperty::VALUE_TYPE_FLOAT; 181 | else if (valueType == Integer) 182 | message.value_type = semantic_map_msgs::DataProperty::VALUE_TYPE_INT; 183 | 184 | message.value = getValue(); 185 | 186 | return message; 187 | } 188 | 189 | } 190 | -------------------------------------------------------------------------------- /semantic_map_common/src/Entity.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "semantic_map_common/Entity.h" 26 | 27 | namespace semantic_map { 28 | 29 | /*****************************************************************************/ 30 | /* Constructors and Destructor */ 31 | /*****************************************************************************/ 32 | 33 | Entity::Entity() { 34 | } 35 | 36 | Entity::Entity(const Entity& src) : 37 | impl_(src.impl_) { 38 | } 39 | 40 | Entity::~Entity() { 41 | } 42 | 43 | Entity::Impl::Impl(const std::string& identifier, const std::string& type, 44 | const Entity& parent) : 45 | identifier_(identifier), 46 | type_(type), 47 | parent_(new Entity(parent)) { 48 | BOOST_ASSERT(!identifier.empty()); 49 | BOOST_ASSERT(!type.empty()); 50 | } 51 | 52 | // Entity::Impl::Impl(const XmlRpc::XmlRpcValue& value, const Entity& parent) : 53 | // parent_(new Entity(parent)) { 54 | // std::string identifier, type; 55 | // 56 | // try { 57 | // identifier = (std::string)const_cast(value)["id"]; 58 | // type = (std::string)const_cast(value)["type"]; 59 | // } 60 | // catch (const XmlRpc::XmlRpcException& exception) { 61 | // throw XmlRpcConversionFailed(exception.getMessage()); 62 | // } 63 | // 64 | // BOOST_ASSERT(!identifier.empty()); 65 | // BOOST_ASSERT(!type.empty()); 66 | // 67 | // const_cast(identifier_) = identifier; 68 | // const_cast(type_) = type; 69 | // } 70 | 71 | Entity::Impl::~Impl() { 72 | } 73 | 74 | /*****************************************************************************/ 75 | /* Accessors */ 76 | /*****************************************************************************/ 77 | 78 | std::string Entity::getIdentifier() const { 79 | if (impl_.get()) 80 | return impl_->identifier_; 81 | else 82 | return std::string(); 83 | } 84 | 85 | void Entity::setType(const std::string& type) { 86 | BOOST_ASSERT(!type.empty()); 87 | 88 | if (impl_.get()) 89 | impl_->type_ = type; 90 | } 91 | 92 | std::string Entity::getType() const { 93 | if (impl_.get()) 94 | return impl_->type_; 95 | else 96 | return std::string(); 97 | } 98 | 99 | Entity Entity::getParent() const { 100 | if (impl_.get()) 101 | return *impl_->parent_; 102 | else 103 | return Entity(); 104 | } 105 | 106 | size_t Entity::getNumProperties() const { 107 | if (impl_.get()) 108 | return impl_->properties_.size(); 109 | else 110 | return 0; 111 | } 112 | 113 | boost::unordered_multimap Entity::getProperties() 114 | const { 115 | if (impl_.get()) 116 | return impl_->properties_; 117 | else 118 | return boost::unordered_multimap(); 119 | } 120 | 121 | std::list Entity::getProperties(const std::string& identifier) 122 | const { 123 | std::list properties; 124 | 125 | if (impl_.get()) { 126 | std::pair:: 127 | const_iterator, boost::unordered_multimap:: 128 | const_iterator> range = impl_->properties_.equal_range(identifier); 129 | 130 | for (boost::unordered_multimap::const_iterator 131 | jt = range.first; jt != range.second; ++jt) 132 | properties.push_back(jt->second); 133 | } 134 | 135 | return properties; 136 | } 137 | 138 | bool Entity::hasParent() const { 139 | if (impl_.get()) 140 | return impl_->parent_.get(); 141 | else 142 | return false; 143 | } 144 | 145 | bool Entity::isMap() const { 146 | if (impl_.get()) 147 | return boost::dynamic_pointer_cast(impl_).get(); 148 | else 149 | return false; 150 | } 151 | 152 | bool Entity::isMission() const { 153 | if (impl_.get()) 154 | return boost::dynamic_pointer_cast(impl_).get(); 155 | else 156 | return false; 157 | } 158 | 159 | bool Entity::isObject() const { 160 | if (impl_.get()) 161 | return boost::dynamic_pointer_cast(impl_).get(); 162 | else 163 | return false; 164 | } 165 | 166 | bool Entity::isAction() const { 167 | if (impl_.get()) 168 | return boost::dynamic_pointer_cast(impl_).get(); 169 | else 170 | return false; 171 | } 172 | 173 | bool Entity::isValid() const { 174 | return impl_.get(); 175 | } 176 | 177 | /*****************************************************************************/ 178 | /* Methods */ 179 | /*****************************************************************************/ 180 | 181 | ObjectProperty Entity::addProperty(const std::string& identifier, const 182 | Entity& object) { 183 | if (impl_.get()) { 184 | ObjectProperty property(identifier, *this, object); 185 | 186 | impl_->properties_.insert(std::make_pair(identifier, property)); 187 | 188 | return property; 189 | } 190 | else 191 | return ObjectProperty(); 192 | } 193 | 194 | void Entity::clearProperties() { 195 | if (impl_.get()) 196 | impl_->properties_.clear(); 197 | } 198 | 199 | // XmlRpc::XmlRpcValue Entity::toXmlRpcValue() const { 200 | // XmlRpc::XmlRpcValue value; 201 | // 202 | // if (impl_.get()) { 203 | // try { 204 | // value["id"] = getIdentifier(); 205 | // value["type"] = getType(); 206 | // } 207 | // catch (const XmlRpc::XmlRpcException& exception) { 208 | // throw XmlRpcConversionFailed(exception.getMessage()); 209 | // } 210 | // } 211 | // 212 | // return value; 213 | // } 214 | 215 | /*****************************************************************************/ 216 | /* Operators */ 217 | /*****************************************************************************/ 218 | 219 | bool Entity::operator==(const Entity& entity) const { 220 | if (isValid() && entity.isValid()) { 221 | if ((getIdentifier() == entity.getIdentifier()) && (getType() == src.getType())) ; 222 | } 223 | else 224 | return (isValid() == entity.isValid()); 225 | } 226 | 227 | } 228 | -------------------------------------------------------------------------------- /semantic_map_common/src/Exceptions.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_common/Exceptions.h" 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Constructors and Destructor */ 25 | /*****************************************************************************/ 26 | 27 | XmlRpcConversionFailed::XmlRpcConversionFailed(const std::string& 28 | description) : 29 | ros::Exception("XML-RPC value conversion failed: "+description) { 30 | } 31 | 32 | } 33 | -------------------------------------------------------------------------------- /semantic_map_common/src/Mission.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_common/Mission.h" 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Constructors and Destructor */ 25 | /*****************************************************************************/ 26 | 27 | Mission::Mission() { 28 | } 29 | 30 | Mission::Mission(const std::string& identifier, const std::string& type, 31 | const std::string& map, const std::string& ns) { 32 | impl_.reset(new Impl(identifier, type, map, ns)); 33 | } 34 | 35 | Mission::Mission(const Mission& src) : 36 | Entity(src) { 37 | } 38 | 39 | Mission::Mission(const Entity& src) : 40 | Entity(src) { 41 | if (impl_.get()) 42 | BOOST_ASSERT(boost::dynamic_pointer_cast(impl_).get()); 43 | } 44 | 45 | Mission::~Mission() { 46 | } 47 | 48 | Mission::Impl::Impl(const std::string& identifier, const std::string& type, 49 | const std::string& map, const std::string& ns) : 50 | Entity::Impl(identifier, type), 51 | ontology_(ns), 52 | map_(map) { 53 | } 54 | 55 | Mission::Impl::~Impl() { 56 | } 57 | 58 | /*****************************************************************************/ 59 | /* Accessors */ 60 | /*****************************************************************************/ 61 | 62 | void Mission::setOntology(const Ontology& ontology) { 63 | if (impl_.get()) 64 | boost::static_pointer_cast(impl_)->ontology_ = ontology; 65 | } 66 | 67 | Ontology Mission::getOntology() const { 68 | if (impl_.get()) 69 | return boost::static_pointer_cast(impl_)->ontology_; 70 | else 71 | return Ontology(); 72 | } 73 | 74 | void Mission::setMap(const std::string& map) { 75 | if (impl_.get()) 76 | boost::static_pointer_cast(impl_)->map_ = map; 77 | } 78 | 79 | std::string Mission::getMap() const { 80 | if (impl_.get()) 81 | return boost::static_pointer_cast(impl_)->map_; 82 | else 83 | return std::string(); 84 | } 85 | 86 | size_t Mission::getNumTasks() const { 87 | if (impl_.get()) 88 | return boost::static_pointer_cast(impl_)->tasks_.size(); 89 | else 90 | return 0; 91 | } 92 | 93 | std::list Mission::getTasks() const { 94 | if (impl_.get()) 95 | return boost::static_pointer_cast(impl_)->tasks_; 96 | else 97 | return std::list(); 98 | } 99 | 100 | /*****************************************************************************/ 101 | /* Methods */ 102 | /*****************************************************************************/ 103 | 104 | std::list::iterator Mission::begin() { 105 | if (impl_.get()) 106 | return boost::static_pointer_cast(impl_)->tasks_.begin(); 107 | else 108 | return std::list::iterator(); 109 | } 110 | 111 | std::list::const_iterator Mission::begin() const { 112 | if (impl_.get()) 113 | return boost::static_pointer_cast(impl_)->tasks_.begin(); 114 | else 115 | return std::list::const_iterator(); 116 | } 117 | 118 | std::list::iterator Mission::end() { 119 | if (impl_.get()) 120 | return boost::static_pointer_cast(impl_)->tasks_.end(); 121 | else 122 | return std::list::iterator(); 123 | } 124 | 125 | std::list::const_iterator Mission::end() const { 126 | if (impl_.get()) 127 | return boost::static_pointer_cast(impl_)->tasks_.end(); 128 | else 129 | return std::list::const_iterator(); 130 | } 131 | 132 | Task Mission::addTask(const std::string& identifier, const std::string& 133 | type, bool asserted, Task::Quantification quantification, bool 134 | unordered) { 135 | if (impl_.get()) { 136 | Task task; 137 | 138 | task.impl_.reset(new Task::Impl(identifier, type, asserted, 139 | quantification, unordered)); 140 | boost::static_pointer_cast(impl_)->tasks_.push_back(task); 141 | 142 | return task; 143 | } 144 | else 145 | return Task(); 146 | } 147 | 148 | void Mission::clearTasks() { 149 | if (impl_.get()) 150 | boost::static_pointer_cast(impl_)->tasks_.clear(); 151 | } 152 | 153 | } 154 | -------------------------------------------------------------------------------- /semantic_map_common/src/NamespacePrefix.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_common/NamespacePrefix.h" 22 | 23 | namespace semantic_map { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | NamespacePrefix::XmlRpcConversionFailed::XmlRpcConversionFailed(const 30 | std::string& description) : 31 | ros::Exception("XML-RPC value conversion of semantic map namespace " 32 | "prefix failed: "+description) { 33 | } 34 | 35 | NamespacePrefix::NamespacePrefix() { 36 | } 37 | 38 | NamespacePrefix::NamespacePrefix(const std::string& prefix, const 39 | std::string& ns) : 40 | impl_(new Impl(prefix, ns)) { 41 | } 42 | 43 | NamespacePrefix::NamespacePrefix(const NamespacePrefix& src) : 44 | impl_(src.impl_) { 45 | } 46 | 47 | NamespacePrefix::~NamespacePrefix() { 48 | } 49 | 50 | NamespacePrefix::Impl::Impl(const std::string& prefix, const 51 | std::string& ns) : 52 | prefix_(prefix), 53 | ns_(ns) { 54 | BOOST_ASSERT(!prefix.empty()); 55 | BOOST_ASSERT(!ns.empty()); 56 | } 57 | 58 | NamespacePrefix::Impl::~Impl() { 59 | } 60 | 61 | /*****************************************************************************/ 62 | /* Accessors */ 63 | /*****************************************************************************/ 64 | 65 | std::string NamespacePrefix::getPrefix() const { 66 | if (impl_.get()) 67 | return impl_->prefix_; 68 | else 69 | return std::string(); 70 | } 71 | 72 | void NamespacePrefix::setNamespace(const std::string& ns) { 73 | BOOST_ASSERT(!ns.empty()); 74 | 75 | if (impl_.get()) 76 | impl_->ns_ = ns; 77 | } 78 | 79 | std::string NamespacePrefix::getNamespace() const { 80 | if (impl_.get()) 81 | return impl_->ns_; 82 | else 83 | return std::string(); 84 | } 85 | 86 | bool NamespacePrefix::isValid() const { 87 | return impl_.get(); 88 | } 89 | 90 | /*****************************************************************************/ 91 | /* Methods */ 92 | /*****************************************************************************/ 93 | 94 | void NamespacePrefix::fromXmlRpcValue(const XmlRpc::XmlRpcValue& value) { 95 | try { 96 | std::string prefix = (std::string)const_cast( 97 | value)["prefix"]; 98 | std::string ns = (std::string)const_cast( 99 | value)["ns"]; 100 | 101 | if (impl_.get()) { 102 | BOOST_ASSERT(prefix == getPrefix()); 103 | setNamespace(ns); 104 | } 105 | else 106 | impl_.reset(new Impl(prefix, ns)); 107 | } 108 | catch (const XmlRpc::XmlRpcException& exception) { 109 | throw XmlRpcConversionFailed(exception.getMessage()); 110 | } 111 | } 112 | 113 | void NamespacePrefix::toXmlRpcValue(XmlRpc::XmlRpcValue& value) const { 114 | if (impl_.get()) { 115 | try { 116 | value["prefix"] = getPrefix(); 117 | value["ns"] = getNamespace(); 118 | } 119 | catch (const XmlRpc::XmlRpcException& exception) { 120 | throw XmlRpcConversionFailed(exception.getMessage()); 121 | } 122 | } 123 | } 124 | 125 | void NamespacePrefix::fromMessage(const semantic_map_msgs::NamespacePrefix& 126 | message) { 127 | if (impl_.get()) { 128 | BOOST_ASSERT(message.prefix == getPrefix()); 129 | setNamespace(message.ns); 130 | } 131 | else 132 | impl_.reset(new Impl(message.prefix, message.ns)); 133 | } 134 | 135 | semantic_map_msgs::NamespacePrefix NamespacePrefix::toMessage() const { 136 | semantic_map_msgs::NamespacePrefix message; 137 | 138 | message.prefix = getPrefix(); 139 | message.ns = getNamespace(); 140 | 141 | return message; 142 | } 143 | 144 | } 145 | -------------------------------------------------------------------------------- /semantic_map_common/src/ObjectProperty.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_common/ObjectProperty.h" 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Constructors and Destructor */ 25 | /*****************************************************************************/ 26 | 27 | ObjectProperty::XmlRpcConversionFailed::XmlRpcConversionFailed(const 28 | std::string& description) : 29 | ros::Exception("XML-RPC value conversion of semantic map object property " 30 | "failed: "+description) { 31 | } 32 | 33 | ObjectProperty::ObjectProperty() { 34 | } 35 | 36 | ObjectProperty::ObjectProperty(const std::string& identifier, const 37 | Entity& subject, const Entity& object) { 38 | impl_.reset(new Impl(identifier, subject, object)); 39 | } 40 | 41 | ObjectProperty::ObjectProperty(const XmlRpc::XmlRpcValue& value, const 42 | boost::unordered_map& entities) { 43 | impl_.reset(new Impl(value, entities)); 44 | } 45 | 46 | ObjectProperty::ObjectProperty(const semantic_map_msgs::ObjectProperty& 47 | message, const boost::unordered_map& entities) { 48 | impl_.reset(new Impl(message, entities)); 49 | } 50 | 51 | ObjectProperty::ObjectProperty(const ObjectProperty& src) : 52 | Property(src) { 53 | } 54 | 55 | ObjectProperty::ObjectProperty(const Property& src) : 56 | Property(src) { 57 | if (impl_.get()) 58 | BOOST_ASSERT(boost::dynamic_pointer_cast(impl_).get()); 59 | } 60 | 61 | ObjectProperty::~ObjectProperty() { 62 | } 63 | 64 | ObjectProperty::Impl::Impl(const std::string& identifier, const Entity& 65 | subject, const Entity& object) : 66 | Property::Impl(identifier, subject), 67 | object_(object) { 68 | BOOST_ASSERT(object.isValid()); 69 | } 70 | 71 | ObjectProperty::Impl::Impl(const XmlRpc::XmlRpcValue& value, const 72 | boost::unordered_map& entities) : 73 | Property::Impl(value, entities) { 74 | std::string object; 75 | 76 | try { 77 | object = (std::string)const_cast(value)["object"]; 78 | } 79 | catch (const XmlRpc::XmlRpcException& exception) { 80 | throw XmlRpcConversionFailed(exception.getMessage()); 81 | } 82 | 83 | boost::unordered_map::const_iterator it = entities. 84 | find(object); 85 | 86 | BOOST_ASSERT(it != entities.end()); 87 | BOOST_ASSERT(it->second.isValid()); 88 | 89 | object_ = it->second; 90 | } 91 | 92 | ObjectProperty::Impl::Impl(const semantic_map_msgs::ObjectProperty& 93 | message, const boost::unordered_map& entities) : 94 | Property::Impl(message, entities) { 95 | boost::unordered_map::const_iterator it = entities. 96 | find(message.object); 97 | 98 | BOOST_ASSERT(it != entities.end()); 99 | BOOST_ASSERT(it->second.isValid()); 100 | 101 | object_ = it->second; 102 | } 103 | 104 | ObjectProperty::Impl::~Impl() { 105 | } 106 | 107 | /*****************************************************************************/ 108 | /* Accessors */ 109 | /*****************************************************************************/ 110 | 111 | void ObjectProperty::setObject(const Entity& object) { 112 | BOOST_ASSERT(object.isValid()); 113 | 114 | if (impl_.get()) 115 | boost::static_pointer_cast(impl_)->object_ = object; 116 | } 117 | 118 | Entity ObjectProperty::getObject() const { 119 | if (impl_.get()) 120 | return boost::static_pointer_cast(impl_)->object_; 121 | else 122 | return Entity(); 123 | } 124 | 125 | /*****************************************************************************/ 126 | /* Methods */ 127 | /*****************************************************************************/ 128 | 129 | XmlRpc::XmlRpcValue ObjectProperty::toXmlRpcValue() const { 130 | XmlRpc::XmlRpcValue value = Property::toXmlRpcValue(); 131 | 132 | if (impl_.get()) { 133 | try { 134 | value["object"] = getObject().getIdentifier(); 135 | } 136 | catch (const XmlRpc::XmlRpcException& exception) { 137 | throw XmlRpcConversionFailed(exception.getMessage()); 138 | } 139 | } 140 | 141 | return value; 142 | } 143 | 144 | semantic_map_msgs::ObjectProperty ObjectProperty::toMessage() const { 145 | semantic_map_msgs::ObjectProperty message = Property:: 146 | toMessage(); 147 | 148 | message.object = getObject().getIdentifier(); 149 | 150 | return message; 151 | } 152 | 153 | } 154 | -------------------------------------------------------------------------------- /semantic_map_common/src/Ontology.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_common/Ontology.h" 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Constructors and Destructor */ 25 | /*****************************************************************************/ 26 | 27 | Ontology::Ontology() { 28 | } 29 | 30 | Ontology::Ontology(const std::string& ns) { 31 | impl_.reset(new Impl(ns)); 32 | } 33 | 34 | Ontology::Ontology(const Ontology& src) : 35 | impl_(src.impl_) { 36 | } 37 | 38 | Ontology::~Ontology() { 39 | } 40 | 41 | Ontology::Impl::Impl(const std::string& ns) : 42 | ns_(ns) { 43 | } 44 | 45 | Ontology::Impl::~Impl() { 46 | } 47 | 48 | /*****************************************************************************/ 49 | /* Accessors */ 50 | /*****************************************************************************/ 51 | 52 | void Ontology::setNamespace(const std::string& ns) { 53 | if (impl_.get()) 54 | impl_->ns_ = ns; 55 | } 56 | 57 | std::string Ontology::getNamespace() const { 58 | if (impl_.get()) 59 | return impl_->ns_; 60 | else 61 | return std::string(); 62 | } 63 | 64 | size_t Ontology::getNumImports() const { 65 | if (impl_.get()) 66 | return impl_->imports_.size(); 67 | else 68 | return 0; 69 | } 70 | 71 | std::list Ontology::getImports() const { 72 | if (impl_.get()) 73 | return impl_->imports_; 74 | else 75 | return std::list(); 76 | } 77 | 78 | size_t Ontology::getNumPrefixes() const { 79 | if (impl_.get()) 80 | return impl_->prefixes_.size(); 81 | else 82 | return 0; 83 | } 84 | 85 | boost::unordered_map Ontology::getPrefixes() 86 | const { 87 | if (impl_.get()) 88 | return impl_->prefixes_; 89 | else 90 | return boost::unordered_map(); 91 | } 92 | 93 | NamespacePrefix Ontology::getPrefix(const std::string& prefix) const { 94 | if (impl_.get()) { 95 | boost::unordered_map::const_iterator it = 96 | impl_->prefixes_.find(prefix); 97 | 98 | if (it != impl_->prefixes_.end()) 99 | return it->second; 100 | } 101 | 102 | return NamespacePrefix(); 103 | } 104 | 105 | bool Ontology::isValid() const { 106 | return impl_.get(); 107 | } 108 | 109 | /*****************************************************************************/ 110 | /* Methods */ 111 | /*****************************************************************************/ 112 | 113 | void Ontology::addImport(const std::string& import) { 114 | BOOST_ASSERT(!import.empty()); 115 | 116 | if (impl_.get()) 117 | impl_->imports_.push_back(import); 118 | } 119 | 120 | void Ontology::clearImports() { 121 | if (impl_.get()) 122 | impl_->imports_.clear(); 123 | } 124 | 125 | void Ontology::addPrefix(const NamespacePrefix& prefix) { 126 | BOOST_ASSERT(prefix.isValid()); 127 | 128 | if (impl_.get()) 129 | impl_->prefixes_.insert(std::make_pair(prefix.getPrefix(), prefix)); 130 | } 131 | 132 | NamespacePrefix Ontology::addPrefix(const std::string& prefix, const 133 | std::string& ns) { 134 | NamespacePrefix namespacePrefix(prefix, ns); 135 | 136 | addPrefix(namespacePrefix); 137 | 138 | return namespacePrefix; 139 | } 140 | 141 | void Ontology::clearPrefixes() { 142 | if (impl_.get()) 143 | impl_->prefixes_.clear(); 144 | } 145 | 146 | } 147 | -------------------------------------------------------------------------------- /semantic_map_common/src/Pose.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_common/Pose.h" 22 | 23 | namespace semantic_map { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | Pose::XmlRpcConversionFailed::XmlRpcConversionFailed(const std::string& 30 | description) : 31 | ros::Exception("XML-RPC value conversion of semantic map pose failed: "+ 32 | description) { 33 | } 34 | 35 | Pose::Pose(double x, double y, double z, double i, double j, double k, 36 | double w) { 37 | position_[0] = x; 38 | position_[1] = y; 39 | position_[2] = z; 40 | 41 | orientation_.x() = i; 42 | orientation_.y() = j; 43 | orientation_.z() = k; 44 | orientation_.w() = w; 45 | } 46 | 47 | Pose::Pose(const XmlRpc::XmlRpcValue& value) { 48 | position_.setZero(); 49 | orientation_.setIdentity(); 50 | 51 | try { 52 | if (value.hasMember("position")) { 53 | XmlRpc::XmlRpcValue& position = const_cast( 54 | value)["position"]; 55 | 56 | if (position.hasMember("x")) 57 | position_[0] = position["x"]; 58 | 59 | if (position.hasMember("y")) 60 | position_[1] = position["y"]; 61 | 62 | if (position.hasMember("z")) 63 | position_[2] = position["z"]; 64 | } 65 | 66 | if (value.hasMember("orientation")) { 67 | XmlRpc::XmlRpcValue& orientation = const_cast( 68 | value)["orientation"]; 69 | 70 | if (orientation.hasMember("x")) 71 | orientation_.x() = orientation["x"]; 72 | 73 | if (orientation.hasMember("y")) 74 | orientation_.y() = orientation["y"]; 75 | 76 | if (orientation.hasMember("z")) 77 | orientation_.z() = orientation["z"]; 78 | 79 | if (orientation.hasMember("w")) 80 | orientation_.w() = orientation["w"]; 81 | 82 | orientation_.normalize(); 83 | } 84 | } 85 | catch (const XmlRpc::XmlRpcException& exception) { 86 | throw XmlRpcConversionFailed(exception.getMessage()); 87 | } 88 | } 89 | 90 | Pose::Pose(const geometry_msgs::Pose& message) { 91 | position_[0] = message.position.x; 92 | position_[1] = message.position.y; 93 | position_[2] = message.position.z; 94 | 95 | orientation_.x() = message.orientation.x; 96 | orientation_.y() = message.orientation.y; 97 | orientation_.z() = message.orientation.z; 98 | orientation_.w() = message.orientation.w; 99 | } 100 | 101 | Pose::Pose(const Pose& src) : 102 | position_(src.position_), 103 | orientation_(src.orientation_) { 104 | } 105 | 106 | Pose::~Pose() { 107 | } 108 | 109 | /*****************************************************************************/ 110 | /* Accessors */ 111 | /*****************************************************************************/ 112 | 113 | void Pose::setPosition(const Eigen::Vector3d& position) { 114 | position_ = position; 115 | } 116 | 117 | const Eigen::Vector3d& Pose::getPosition() const { 118 | return position_; 119 | } 120 | 121 | void Pose::setOrientation(const Eigen::Quaterniond& orientation) { 122 | orientation_ = orientation; 123 | } 124 | 125 | const Eigen::Quaterniond& Pose::getOrientation() const { 126 | return orientation_; 127 | } 128 | 129 | /*****************************************************************************/ 130 | /* Methods */ 131 | /*****************************************************************************/ 132 | 133 | XmlRpc::XmlRpcValue Pose::toXmlRpcValue() const { 134 | XmlRpc::XmlRpcValue value; 135 | 136 | try { 137 | value["position"]["x"] = position_[0]; 138 | value["position"]["y"] = position_[1]; 139 | value["position"]["z"] = position_[2]; 140 | 141 | value["orientation"]["x"] = orientation_.x(); 142 | value["orientation"]["y"] = orientation_.y(); 143 | value["orientation"]["z"] = orientation_.z(); 144 | value["orientation"]["w"] = orientation_.w(); 145 | } 146 | catch (const XmlRpc::XmlRpcException& exception) { 147 | throw XmlRpcConversionFailed(exception.getMessage()); 148 | } 149 | 150 | return value; 151 | } 152 | 153 | geometry_msgs::Pose Pose::toMessage() const { 154 | geometry_msgs::Pose message; 155 | 156 | message.position.x = position_[0]; 157 | message.position.y = position_[1]; 158 | message.position.z = position_[2]; 159 | 160 | message.orientation.x = orientation_.x(); 161 | message.orientation.y = orientation_.y(); 162 | message.orientation.z = orientation_.z(); 163 | message.orientation.w = orientation_.w(); 164 | 165 | return message; 166 | } 167 | 168 | } 169 | -------------------------------------------------------------------------------- /semantic_map_common/src/Property.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "semantic_map_common/Property.h" 26 | 27 | namespace semantic_map { 28 | 29 | /*****************************************************************************/ 30 | /* Constructors and Destructor */ 31 | /*****************************************************************************/ 32 | 33 | Property::XmlRpcConversionFailed::XmlRpcConversionFailed(const std::string& 34 | description) : 35 | ros::Exception("XML-RPC value conversion of semantic map property failed: "+ 36 | description) { 37 | } 38 | 39 | Property::Property() { 40 | } 41 | 42 | Property::Property(const Property& src) : 43 | impl_(src.impl_) { 44 | } 45 | 46 | Property::~Property() { 47 | } 48 | 49 | Property::Impl::Impl(const std::string& identifier, const Entity& subject) : 50 | identifier_(identifier), 51 | subject_(new Entity(subject)) { 52 | BOOST_ASSERT(!identifier.empty()); 53 | BOOST_ASSERT(subject.isValid()); 54 | } 55 | 56 | Property::Impl::Impl(const XmlRpc::XmlRpcValue& value, const boost:: 57 | unordered_map& entities) { 58 | std::string identifier, subject; 59 | 60 | try { 61 | identifier = (std::string)const_cast(value)["id"]; 62 | subject = (std::string)const_cast(value)["subject"]; 63 | } 64 | catch (const XmlRpc::XmlRpcException& exception) { 65 | throw XmlRpcConversionFailed(exception.getMessage()); 66 | } 67 | 68 | BOOST_ASSERT(!identifier.empty()); 69 | 70 | boost::unordered_map::const_iterator it = entities. 71 | find(subject); 72 | 73 | BOOST_ASSERT(it != entities.end()); 74 | BOOST_ASSERT(it->second.isValid()); 75 | 76 | const_cast(identifier_) = identifier; 77 | const_cast&>(subject_).reset( 78 | new Entity(it->second)); 79 | } 80 | 81 | 82 | Property::Impl::~Impl() { 83 | } 84 | 85 | /*****************************************************************************/ 86 | /* Accessors */ 87 | /*****************************************************************************/ 88 | 89 | std::string Property::getIdentifier() const { 90 | if (impl_.get()) 91 | return impl_->identifier_; 92 | else 93 | return std::string(); 94 | } 95 | 96 | Entity Property::getSubject() const { 97 | if (impl_.get()) 98 | return *impl_->subject_; 99 | else 100 | return Entity(); 101 | } 102 | 103 | bool Property::isDataProperty() const { 104 | if (impl_.get()) 105 | return boost::dynamic_pointer_cast(impl_).get(); 106 | else 107 | return false; 108 | } 109 | 110 | bool Property::isObjectProperty() const { 111 | if (impl_.get()) 112 | return boost::dynamic_pointer_cast(impl_).get(); 113 | else 114 | return false; 115 | } 116 | 117 | bool Property::isValid() const { 118 | return impl_.get(); 119 | } 120 | 121 | /*****************************************************************************/ 122 | /* Methods */ 123 | /*****************************************************************************/ 124 | 125 | XmlRpc::XmlRpcValue Property::toXmlRpcValue() const { 126 | XmlRpc::XmlRpcValue value; 127 | 128 | if (impl_.get()) { 129 | try { 130 | value["id"] = getIdentifier(); 131 | value["subject"] = getSubject().getIdentifier(); 132 | } 133 | catch (const XmlRpc::XmlRpcException& exception) { 134 | throw XmlRpcConversionFailed(exception.getMessage()); 135 | } 136 | } 137 | 138 | return value; 139 | } 140 | 141 | } 142 | -------------------------------------------------------------------------------- /semantic_map_common/src/Size.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_common/Size.h" 22 | 23 | namespace semantic_map { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | Size::XmlRpcConversionFailed::XmlRpcConversionFailed(const std::string& 30 | description) : 31 | ros::Exception("XML-RPC value conversion of semantic map size failed: "+ 32 | description) { 33 | } 34 | 35 | Size::Size(double width, double height, double depth) { 36 | dimensions_[0] = depth; 37 | dimensions_[1] = width; 38 | dimensions_[2] = height; 39 | } 40 | 41 | Size::Size(const XmlRpc::XmlRpcValue& value) { 42 | dimensions_.setZero(); 43 | 44 | try { 45 | if (value.hasMember("x")) 46 | dimensions_[0] = const_cast(value)["x"]; 47 | 48 | if (value.hasMember("y")) 49 | dimensions_[1] = const_cast(value)["y"]; 50 | 51 | if (value.hasMember("z")) 52 | dimensions_[2] = const_cast(value)["z"]; 53 | } 54 | catch (const XmlRpc::XmlRpcException& exception) { 55 | throw XmlRpcConversionFailed(exception.getMessage()); 56 | } 57 | } 58 | 59 | Size::Size(const semantic_map_msgs::Size& message) { 60 | dimensions_[0] = message.x; 61 | dimensions_[1] = message.y; 62 | dimensions_[2] = message.z; 63 | } 64 | 65 | Size::Size(const Size& src) : 66 | dimensions_(src.dimensions_) { 67 | } 68 | 69 | Size::~Size() { 70 | } 71 | 72 | /*****************************************************************************/ 73 | /* Accessors */ 74 | /*****************************************************************************/ 75 | 76 | void Size::setWidth(double width) { 77 | dimensions_[1] = width; 78 | } 79 | 80 | double Size::getWidth() const { 81 | return dimensions_[1]; 82 | } 83 | 84 | void Size::setHeight(double height) { 85 | dimensions_[2] = height; 86 | } 87 | 88 | double Size::getHeight() const { 89 | return dimensions_[2]; 90 | } 91 | 92 | void Size::setDepth(double depth) { 93 | dimensions_[0] = depth; 94 | } 95 | 96 | double Size::getDepth() const { 97 | return dimensions_[0]; 98 | } 99 | 100 | void Size::setDimensions(const Eigen::Vector3d& dimensions) { 101 | dimensions_ = dimensions; 102 | } 103 | 104 | const Eigen::Vector3d& Size::getDimensions() const { 105 | return dimensions_; 106 | } 107 | 108 | /*****************************************************************************/ 109 | /* Methods */ 110 | /*****************************************************************************/ 111 | 112 | XmlRpc::XmlRpcValue Size::toXmlRpcValue() const { 113 | XmlRpc::XmlRpcValue value; 114 | 115 | try { 116 | value["x"] = dimensions_[0]; 117 | value["y"] = dimensions_[1]; 118 | value["z"] = dimensions_[2]; 119 | } 120 | catch (const XmlRpc::XmlRpcException& exception) { 121 | throw XmlRpcConversionFailed(exception.getMessage()); 122 | } 123 | 124 | return value; 125 | } 126 | 127 | semantic_map_msgs::Size Size::toMessage() const { 128 | semantic_map_msgs::Size message; 129 | 130 | message.x = dimensions_[0]; 131 | message.y = dimensions_[1]; 132 | message.z = dimensions_[2]; 133 | 134 | return message; 135 | } 136 | 137 | } 138 | -------------------------------------------------------------------------------- /semantic_map_common/src/Task.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_common/Task.h" 20 | 21 | namespace semantic_map { 22 | 23 | /*****************************************************************************/ 24 | /* Constructors and Destructor */ 25 | /*****************************************************************************/ 26 | 27 | Task::Task() { 28 | } 29 | 30 | Task::Task(const Task& src) : 31 | Action(src) { 32 | } 33 | 34 | Task::Task(const Entity& src) : 35 | Action(src) { 36 | if (impl_.get()) 37 | BOOST_ASSERT(boost::dynamic_pointer_cast(impl_).get()); 38 | } 39 | 40 | Task::~Task() { 41 | } 42 | 43 | Task::Impl::Impl(const std::string& identifier, const std::string& type, 44 | bool asserted, Quantification quantification, bool unordered) : 45 | Action::Impl(identifier, type, asserted), 46 | quantification_(quantification), 47 | unordered_(unordered) { 48 | } 49 | 50 | // Task::Impl::Impl(const XmlRpc::XmlRpcValue& value, const std::list& 51 | // actions, const Task& parentTask) : 52 | // Action::Impl(value, parentTask) { 53 | // std::vector actionIdentifiers; 54 | // std::string quantification; 55 | // bool unordered; 56 | // 57 | // try { 58 | // if (value.hasMember("actions")) { 59 | // XmlRpc::XmlRpcValue& actionsValue = const_cast( 60 | // value)["actions"]; 61 | // actionIdentifiers.reserve(actionsValue.size()); 62 | // 63 | // for (size_t index = 0; index < actionsValue.size(); ++index) 64 | // actionIdentifiers.push_back(actionsValue[index]); 65 | // } 66 | // 67 | // quantification = (std::string)const_cast( 68 | // value)["quantification"]; 69 | // unordered = (bool)const_cast(value)["unordered"]; 70 | // } 71 | // catch (const XmlRpc::XmlRpcException& exception) { 72 | // throw XmlRpcConversionFailed(exception.getMessage()); 73 | // } 74 | // 75 | // for (size_t index = 0; index < actionIdentifiers.size(); ++index) { 76 | // boost::unordered_map::const_iterator it = 77 | // actions.find(actionIdentifiers[index]); 78 | // 79 | // BOOST_ASSERT(it != actions.end()); 80 | // BOOST_ASSERT(it->second.isValid()); 81 | // 82 | // actions_.push_back(it->second); 83 | // } 84 | // 85 | // if (quantification == "union") 86 | // quantification_ = Union; 87 | // else 88 | // quantification_ = Intersection; 89 | // 90 | // unordered_ = unordered; 91 | // } 92 | 93 | Task::Impl::~Impl() { 94 | } 95 | 96 | /*****************************************************************************/ 97 | /* Accessors */ 98 | /*****************************************************************************/ 99 | 100 | void Task::setQuantification(Quantification quantification) { 101 | if (impl_.get()) 102 | boost::static_pointer_cast(impl_)->quantification_ = 103 | quantification; 104 | } 105 | 106 | Task::Quantification Task::getQuantification() const { 107 | if (impl_.get()) 108 | return boost::static_pointer_cast(impl_)->quantification_; 109 | else 110 | return Intersection; 111 | } 112 | 113 | size_t Task::getNumActions() const { 114 | if (impl_.get()) 115 | return boost::static_pointer_cast(impl_)->actions_.size(); 116 | else 117 | return 0; 118 | } 119 | 120 | std::list Task::getActions() const { 121 | if (impl_.get()) 122 | return boost::static_pointer_cast(impl_)->actions_; 123 | else 124 | return std::list(); 125 | } 126 | 127 | void Task::setUnordered(bool unordered) { 128 | if (impl_.get()) 129 | boost::static_pointer_cast(impl_)->unordered_ = unordered; 130 | } 131 | 132 | bool Task::isUnordered() const { 133 | if (impl_.get()) 134 | return boost::static_pointer_cast(impl_)->unordered_; 135 | else 136 | return false; 137 | } 138 | 139 | /*****************************************************************************/ 140 | /* Methods */ 141 | /*****************************************************************************/ 142 | 143 | std::list::iterator Task::begin() { 144 | if (impl_.get()) 145 | return boost::static_pointer_cast(impl_)->actions_.begin(); 146 | else 147 | return std::list::iterator(); 148 | } 149 | 150 | std::list::const_iterator Task::begin() const { 151 | if (impl_.get()) 152 | return boost::static_pointer_cast(impl_)->actions_.begin(); 153 | else 154 | return std::list::const_iterator(); 155 | } 156 | 157 | std::list::iterator Task::end() { 158 | if (impl_.get()) 159 | return boost::static_pointer_cast(impl_)->actions_.end(); 160 | else 161 | return std::list::iterator(); 162 | } 163 | 164 | std::list::const_iterator Task::end() const { 165 | if (impl_.get()) 166 | return boost::static_pointer_cast(impl_)->actions_.end(); 167 | else 168 | return std::list::const_iterator(); 169 | } 170 | 171 | void Task::addAction(const Action& action) { 172 | if (impl_.get()) 173 | boost::static_pointer_cast(impl_)->actions_.push_back(action); 174 | } 175 | 176 | Task Task::addTask(const std::string& identifier, const std::string& type, 177 | bool asserted, Quantification quantification, bool unordered) { 178 | if (impl_.get()) { 179 | Task task; 180 | 181 | task.impl_.reset(new Impl(identifier, type, asserted, quantification, 182 | unordered)); 183 | boost::static_pointer_cast(impl_)->actions_.push_back(task); 184 | 185 | return task; 186 | } 187 | else 188 | return Task(); 189 | } 190 | 191 | void Task::clearActions() { 192 | if (impl_.get()) 193 | boost::static_pointer_cast(impl_)->actions_.clear(); 194 | } 195 | 196 | // XmlRpc::XmlRpcValue Task::toXmlRpcValue() const { 197 | // XmlRpc::XmlRpcValue value = Action::toXmlRpcValue(); 198 | // 199 | // if (impl_.get()) { 200 | // try { 201 | // size_t actionIndex = 0; 202 | // for (std::list::const_iterator it = begin(); it != end(); 203 | // ++it, ++actionIndex) 204 | // value["actions"][actionIndex] = it->getIdentifier(); 205 | // 206 | // if (getQuantification() == Union) 207 | // value["quantification"] = "union"; 208 | // else 209 | // value["quantification"] = "intersection"; 210 | // 211 | // value["unordered"] = isUnordered(); 212 | // } 213 | // catch (const XmlRpc::XmlRpcException& exception) { 214 | // throw XmlRpcConversionFailed(exception.getMessage()); 215 | // } 216 | // } 217 | // 218 | // return value; 219 | // } 220 | 221 | } 222 | -------------------------------------------------------------------------------- /semantic_map_conversions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_conversions) 3 | 4 | set(CMAKE_CXX_FLAGS -std=c++11) 5 | 6 | find_package( 7 | catkin 8 | REQUIRED 9 | roscpp 10 | semantic_map_common 11 | semantic_map_msgs 12 | ) 13 | 14 | find_package(PkgConfig) 15 | pkg_check_modules(JSONCPP REQUIRED jsoncpp) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS 19 | include 20 | LIBRARIES 21 | semantic_map_conversions 22 | DEPENDS 23 | roscpp 24 | semantic_map_common 25 | semantic_map_msgs 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | add_library( 34 | semantic_map_conversions 35 | src/MessageConversions.cpp 36 | src/XmlRpcValueConversions.cpp 37 | ) 38 | 39 | target_link_libraries( 40 | semantic_map_conversions 41 | ${catkin_LIBRARIES} 42 | ) 43 | -------------------------------------------------------------------------------- /semantic_map_conversions/include/semantic_map_conversions/MessageConversions.tpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | namespace semantic_map { namespace conversions { 20 | 21 | /*****************************************************************************/ 22 | /* Methods */ 23 | /*****************************************************************************/ 24 | 25 | template M MessageConversions::actionToMessage(const Action& action) 26 | const { 27 | M message = this->template entityToMessage(action); 28 | 29 | message.asserted = action.isAsserted(); 30 | 31 | return message; 32 | } 33 | 34 | template M MessageConversions::entityToMessage(const Entity& entity) 35 | const { 36 | M message; 37 | 38 | message.id = entity.getIdentifier(); 39 | message.type = entity.getType(); 40 | 41 | return message; 42 | } 43 | 44 | template M MessageConversions::propertyToMessage(const Property& 45 | property) const { 46 | M message; 47 | 48 | message.id = property.getIdentifier(); 49 | message.subject = property.getSubject().getIdentifier(); 50 | 51 | return message; 52 | } 53 | 54 | template Task MessageConversions::taskFromMessage(const 55 | semantic_map_msgs::Task& message, P& parent) const { 56 | Task task = parent.addTask(message.id, message.type, message.asserted); 57 | 58 | for (size_t index = 0; index < message.actions.size(); ++index) 59 | ; 60 | // task.addAction(message.actions[index]); 61 | 62 | if (message.quantification == semantic_map_msgs::Task::QUANTIFICATION_UNION) 63 | task.setQuantification(Task::Union); 64 | else 65 | task.setQuantification(Task::Intersection); 66 | 67 | task.setUnordered(message.unordered); 68 | 69 | return task; 70 | } 71 | 72 | }} 73 | -------------------------------------------------------------------------------- /semantic_map_conversions/include/semantic_map_conversions/XmlRpcValueConversions.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file XmlRpcValueConversions.h 20 | * \brief Header file providing the XmlRpcValueConversions class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_XML_RPC_CONVERSIONS_H 24 | #define ROS_SEMANTIC_MAP_XML_RPC_CONVERSIONS_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | namespace semantic_map { 37 | namespace conversions { 38 | /** \brief Semantic map XML-RPC value conversions 39 | */ 40 | class XmlRpcValueConversions { 41 | public: 42 | /** \brief Exception thrown in case of a failure to convert from 43 | * or to an XML-RPC value 44 | */ 45 | class ConversionFailed : 46 | public ros::Exception { 47 | public: 48 | ConversionFailed(const std::string& description); 49 | }; 50 | 51 | /** \brief Default constructor 52 | */ 53 | XmlRpcValueConversions(); 54 | 55 | /** \brief Destructor 56 | */ 57 | virtual ~XmlRpcValueConversions(); 58 | }; 59 | }; 60 | }; 61 | 62 | #include 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /semantic_map_conversions/include/semantic_map_conversions/XmlRpcValueConversions.tpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | namespace semantic_map { namespace conversions { 20 | 21 | /*****************************************************************************/ 22 | /* Methods */ 23 | /*****************************************************************************/ 24 | 25 | }} 26 | -------------------------------------------------------------------------------- /semantic_map_conversions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_conversions 3 | 0.0.1 4 | 5 | Semantic map conversions to and from ROS messages and parameters. 6 | 7 | Ralf Kaestner 8 | 9 | LGPL 10 | 11 | Ralf Kaestner 12 | 13 | catkin 14 | 15 | roscpp 16 | semantic_map_common 17 | semantic_map_msgs 18 | 19 | roscpp 20 | semantic_map_common 21 | semantic_map_msgs 22 | 23 | -------------------------------------------------------------------------------- /semantic_map_conversions/src/XmlRpcValueConversions.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_conversions/XmlRpcValueConversions.h" 22 | 23 | namespace semantic_map { namespace conversions { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | XmlRpcValueConversions::ConversionFailed::ConversionFailed(const std::string& 30 | description) : 31 | ros::Exception("XML-RPC value conversion failed: "+description) { 32 | } 33 | 34 | XmlRpcValueConversions::XmlRpcValueConversions() { 35 | } 36 | 37 | XmlRpcValueConversions::~XmlRpcValueConversions() { 38 | } 39 | 40 | /*****************************************************************************/ 41 | /* Methods */ 42 | /*****************************************************************************/ 43 | 44 | }} 45 | -------------------------------------------------------------------------------- /semantic_map_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_msgs) 3 | 4 | find_package( 5 | catkin 6 | REQUIRED 7 | geometry_msgs 8 | message_generation 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | ActionFeedback.msg 14 | ActionOnObject.msg 15 | Address.msg 16 | DataProperty.msg 17 | Map.msg 18 | Mission.msg 19 | NamespacePrefix.msg 20 | Object.msg 21 | ObjectProperty.msg 22 | Ontology.msg 23 | Size.msg 24 | Task.msg 25 | ) 26 | 27 | add_service_files( 28 | FILES 29 | GetOntology.srv 30 | GetMap.srv 31 | ) 32 | 33 | generate_messages( 34 | DEPENDENCIES geometry_msgs 35 | ) 36 | 37 | catkin_package( 38 | DEPENDS message_runtime 39 | ) 40 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/ActionFeedback.msg: -------------------------------------------------------------------------------- 1 | Header header # action timestamp 2 | 3 | string id # action identifier 4 | string object_acted_on # identifier of the object acted on 5 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/ActionOnObject.msg: -------------------------------------------------------------------------------- 1 | string id # unique action class identifier 2 | string type # action superclass 3 | 4 | bool asserted # if true, this action is asserted 5 | 6 | string object_acted_on # identifier of the object acted on 7 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Address.msg: -------------------------------------------------------------------------------- 1 | string room_nr # room number 2 | string floor_nr # floor number 3 | string street_nr # street number 4 | string street_name # street name 5 | string city_name # city name 6 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/DataProperty.msg: -------------------------------------------------------------------------------- 1 | uint8 VALUE_TYPE_STRING = 0 # value is a string 2 | uint8 VALUE_TYPE_BOOL = 1 # value is a Boolean 3 | uint8 VALUE_TYPE_FLOAT = 2 # value is a float 4 | uint8 VALUE_TYPE_INT = 3 # value is an integer 5 | 6 | string id # data property identifier 7 | string subject # data property subject 8 | uint8 value_type # data property value type (as defined above) 9 | string value # data property value 10 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Map.msg: -------------------------------------------------------------------------------- 1 | Header header # map timestamp and frame id 2 | 3 | Ontology ontology # ontology of this map 4 | 5 | string id # unique map identifier 6 | string type # map type 7 | 8 | Address address # address of this map 9 | 10 | Object[] objects # list of objects in this map 11 | ActionOnObject[] actions # list of actions on the map objects 12 | 13 | ObjectProperty[] object_properties # list of object properties of this map 14 | DataProperty[] data_properties # list of data properties of this map 15 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Mission.msg: -------------------------------------------------------------------------------- 1 | Ontology ontology # ontology of this mission 2 | 3 | string id # unique mission identifier 4 | string type # mission type 5 | 6 | string map # map identifier of this mission 7 | 8 | Task[] tasks # list of tasks of this mission 9 | 10 | ObjectProperty[] object_properties # list of object properties of this mission 11 | DataProperty[] data_properties # list of data properties of this mission 12 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/NamespacePrefix.msg: -------------------------------------------------------------------------------- 1 | string prefix # prefix 2 | string ns # corresponding namespace 3 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Object.msg: -------------------------------------------------------------------------------- 1 | Header header # object timestamp and frame id 2 | 3 | string id # unique object identifier 4 | string type # object type 5 | 6 | string parent # identifier of the parent of this object 7 | 8 | geometry_msgs/Pose pose # object pose 9 | Size size # object size 10 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/ObjectProperty.msg: -------------------------------------------------------------------------------- 1 | string id # object property identifier 2 | string subject # object property subject 3 | string object # object property object 4 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Ontology.msg: -------------------------------------------------------------------------------- 1 | string ns # OWL namespace of this ontology 2 | 3 | string[] imports # OWL imports of this ontology 4 | NamespacePrefix[] ns_prefixes # OWL namespace prefixes of this ontology 5 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Size.msg: -------------------------------------------------------------------------------- 1 | float32 x # depth dimension 2 | float32 y # width dimension 3 | float32 z # height dimension 4 | -------------------------------------------------------------------------------- /semantic_map_msgs/msg/Task.msg: -------------------------------------------------------------------------------- 1 | int8 QUANTIFICATION_INTERSECTION = 0 # conjunction of actions/subtasks (and) 2 | int8 QUANTIFICATION_UNION = 1 # disjunction of actions/subtasks (or) 3 | 4 | string id # unique task class identifier 5 | string type # task superclass 6 | 7 | bool asserted # if true, this task is asserted 8 | 9 | string[] actions # identifiers of the actions/subtasks 10 | int8 quantification # quantification of actions as given above 11 | bool unordered # if true, actions/subtasks are unordered 12 | -------------------------------------------------------------------------------- /semantic_map_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_msgs 3 | 0.0.1 4 | 5 | Message definitions for the ROS semantic map framework. 6 | 7 | Ralf Kaestner 8 | 9 | LGPL 10 | 11 | Ralf Kaestner 12 | 13 | catkin 14 | 15 | message_generation 16 | geometry_msgs 17 | 18 | message_runtime 19 | geometry_msgs 20 | 21 | -------------------------------------------------------------------------------- /semantic_map_msgs/srv/GetMap.srv: -------------------------------------------------------------------------------- 1 | --- 2 | Map map 3 | -------------------------------------------------------------------------------- /semantic_map_msgs/srv/GetOntology.srv: -------------------------------------------------------------------------------- 1 | Map map # Map to retrieve the ontology for 2 | --- 3 | string ontology # Ontological description of the map in OWL format 4 | -------------------------------------------------------------------------------- /semantic_map_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_server) 3 | 4 | set(CMAKE_CXX_FLAGS -std=c++11) 5 | 6 | find_package( 7 | catkin 8 | REQUIRED 9 | nodelet 10 | roscpp 11 | roscpp_nodewrap 12 | semantic_map_msgs 13 | ) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS 17 | include 18 | LIBRARIES 19 | semantic_map_server 20 | DEPENDS 21 | nodelet 22 | roscpp 23 | roscpp_nodewrap 24 | semantic_map_msgs 25 | ) 26 | 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | add_library( 33 | semantic_map_server 34 | src/Server.cpp 35 | ) 36 | 37 | target_link_libraries( 38 | semantic_map_server 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | add_executable( 43 | server_node 44 | src/server_node.cpp 45 | ) 46 | 47 | target_link_libraries( 48 | server_node 49 | semantic_map_server 50 | ${catkin_LIBRARIES} 51 | ) 52 | -------------------------------------------------------------------------------- /semantic_map_server/config/server.yaml: -------------------------------------------------------------------------------- 1 | servers: 2 | get_semantic_map: 3 | service: "get_semantic_map" 4 | -------------------------------------------------------------------------------- /semantic_map_server/include/semantic_map_server/Server.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file Server.h 20 | * \brief Header file providing the Server class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_SERVER_H 24 | #define ROS_SEMANTIC_MAP_SERVER_H 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | namespace semantic_map { 32 | namespace server { 33 | /** \brief Semantic map server implementation 34 | */ 35 | class Server : 36 | public nodewrap::NodeImpl { 37 | public: 38 | /** \brief Default constructor 39 | */ 40 | Server(); 41 | 42 | /** \brief Destructor 43 | */ 44 | virtual ~Server(); 45 | 46 | protected: 47 | /** \brief Initialize the semantic map server 48 | */ 49 | void init(); 50 | 51 | /** \brief Cleanup the semantic map server 52 | */ 53 | void cleanup(); 54 | 55 | private: 56 | /** \brief The service server for retrieving the semantic map 57 | */ 58 | nodewrap::ServiceServer getSemanticMapServer_; 59 | 60 | /** \brief The service callback for retrieving the semantic map 61 | */ 62 | bool getSemanticMapCallback(semantic_map_msgs::GetSemanticMap:: 63 | Request& request, semantic_map_msgs::GetSemanticMap::Response& 64 | response); 65 | }; 66 | }; 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /semantic_map_server/launch/server_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /semantic_map_server/launch/server_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 7 | 8 | 9 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /semantic_map_server/maps/example.yaml: -------------------------------------------------------------------------------- 1 | frame_id: "map" 2 | stamp: 2015-03-12T15:08:00 3 | 4 | namespace: "http://asl.ethz.ch/knowrob/example_semantic_map.owl" 5 | id: "ExampleSemanticMap" 6 | 7 | prefixes: 8 | - name: "marker_interaction" 9 | prefix: "http://asl.ethz.ch/knowrob/marker_interaction.owl#" 10 | imports: 11 | - "package://knowrob_semantic_map_tools/owl/semantic_map.owl" 12 | 13 | address: 14 | room_nr: 224 15 | floor_nr: "J" 16 | street_nr: 21 17 | street_name: "Leonhardstrasse" 18 | city_name: "Zurich" 19 | 20 | object_properties: 21 | hinged_to: 22 | id: "knowrob:hingedTo" 23 | end_state: 24 | id: "knowrob:taskEndState" 25 | 26 | data_properties: 27 | path_to_handle: 28 | id: "marker_interaction:pathToInteractiveMarkerHandle" 29 | menu_entry: 30 | id: "marker_interaction:menuEntryTitle" 31 | purchase_price: 32 | id: "knowrob:purchasePrice" 33 | 34 | objects: 35 | - id: "Cupboard1" 36 | type: "Cupboard" 37 | size: 38 | x: 0.5 39 | y: 0.6 40 | z: 0.7 41 | position: 42 | x: 0 43 | y: 0 44 | z: 0 45 | orientation: 46 | w: 1 47 | x: 0 48 | y: 0 49 | z: 0 50 | purchase_price: 400 51 | parts: 52 | - id: "Door1" 53 | type: "Door" 54 | size: 55 | x: 0.01 56 | y: 0.6 57 | z: 0.7 58 | position: 59 | x: 0.26 60 | y: 0 61 | z: 0 62 | orientation: 63 | w: 1 64 | x: 0 65 | y: 0 66 | z: 0 67 | hinged_to: "Cupboard1" 68 | path_to_handle: "package://knowrob_semantic_map_tools/handles/door.dae" 69 | actions: 70 | - type: "knowrob:OpeningADoor" 71 | asserted: true 72 | end_state: "knowrob:ObjectStateOpen" 73 | menu_entry: "Open %s" 74 | - type: "knowrob:ClosingADoor" 75 | asserted: true 76 | end_state: "knowrob:ObjectStateClosed" 77 | menu_entry: "Close %s" 78 | parts: 79 | - id: "HingedJoint1" 80 | type: "HingedJoint" 81 | size: 82 | x: 0.01 83 | y: 0.007 84 | z: 0.006 85 | position: 86 | x: 0.255 87 | y: -0.3 88 | z: -0.003 89 | orientation: 90 | w: 1 91 | x: 0 92 | y: 0 93 | z: 0 94 | - id: "Handle1" 95 | type: "Handle" 96 | size: 97 | x: 0.05 98 | y: 0.06 99 | z: 0.07 100 | position: 101 | x: 0.26 102 | y: 0 103 | z: 0 104 | orientation: 105 | w: 1 106 | x: 0 107 | y: 0 108 | z: 0 109 | -------------------------------------------------------------------------------- /semantic_map_server/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Semantic map server for ROS. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /semantic_map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_server 3 | 0.0.1 4 | 5 | Semantic map server for ROS. 6 | 7 | Ralf Kaestner 8 | 9 | LGPL 10 | 11 | Ralf Kaestner 12 | 13 | catkin 14 | 15 | nodelet 16 | roscpp 17 | roscpp_nodewrap 18 | semantic_map_msgs 19 | 20 | nodelet 21 | roscpp 22 | roscpp_nodewrap 23 | semantic_map_msgs 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /semantic_map_server/src/Server.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_server/Server.h" 20 | 21 | NODEWRAP_EXPORT_CLASS(semantic_map_server, semantic_map::server::Server) 22 | 23 | namespace semantic_map { namespace server { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | Server::Server() { 30 | } 31 | 32 | Server::~Server() { 33 | } 34 | 35 | /*****************************************************************************/ 36 | /* Methods */ 37 | /*****************************************************************************/ 38 | 39 | void Server::init() { 40 | getSemanticMapServer_ = advertiseService("get_semantic_map", 41 | "get_semantic_map", &Server::getSemanticMapCallback); 42 | } 43 | 44 | void Server::cleanup() { 45 | getSemanticMapServer_.shutdown(); 46 | } 47 | 48 | bool Server::getSemanticMapCallback(semantic_map_msgs::GetSemanticMap:: 49 | Request& request, semantic_map_msgs::GetSemanticMap::Response& 50 | response) { 51 | } 52 | 53 | }} 54 | -------------------------------------------------------------------------------- /semantic_map_server/src/server_node.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_server/Server.h" 22 | 23 | int main(int argc, char** argv) { 24 | ros::init(argc, argv, "semantic_map_server"); 25 | 26 | nodewrap::Node node; 27 | 28 | ros::spin(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /semantic_map_test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_test) 3 | 4 | set(CMAKE_CXX_FLAGS -std=c++11) 5 | 6 | find_package( 7 | catkin 8 | REQUIRED 9 | roscpp 10 | semantic_map_common 11 | semantic_map_conversions 12 | ) 13 | 14 | catkin_package( 15 | DEPENDS 16 | roscpp 17 | semantic_map_common 18 | semantic_map_conversions 19 | ) 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ) 24 | 25 | catkin_add_gtest( 26 | semantic_map_tests 27 | test/semantic_map_tests.cpp 28 | test/CommonTest.cpp 29 | test/ConversionsTest.cpp 30 | ) 31 | 32 | target_link_libraries( 33 | semantic_map_tests 34 | ${catkin_LIBRARIES} 35 | ) 36 | -------------------------------------------------------------------------------- /semantic_map_test/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_test 3 | 0.0.1 4 | 5 | Semantic map testing suite. 6 | 7 | Ralf Kaestner 8 | 9 | LGPL 10 | 11 | Ralf Kaestner 12 | 13 | catkin 14 | 15 | roscpp 16 | semantic_map_common 17 | semantic_map_conversions 18 | 19 | roscpp 20 | semantic_map_common 21 | semantic_map_conversions 22 | 23 | -------------------------------------------------------------------------------- /semantic_map_test/test/CommonTest.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | 23 | using namespace semantic_map; 24 | 25 | TEST(SemanticMapCommon, Map) { 26 | ros::Time::init(); 27 | 28 | Map map("TestMap", "TestMapClass"); 29 | Object object_1 = map.addObject("TestObject1", "TestObjectClass"); 30 | 31 | EXPECT_TRUE(map["TestObject1"].isValid()); 32 | EXPECT_FALSE(map["TestObjectX"].isValid()); 33 | } 34 | -------------------------------------------------------------------------------- /semantic_map_test/test/ConversionsTest.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include 22 | 23 | using namespace semantic_map; 24 | 25 | TEST(SemanticMapConversions, MessageConversions) { 26 | ros::Time::init(); 27 | 28 | Map map("TestMap", "TestMapClass"); 29 | Object object_1 = map.addObject("TestObject1", "TestObjectClass"); 30 | 31 | conversions::MessageConversions conv; 32 | semantic_map_msgs::Map message = conv.mapToMessage(map); 33 | EXPECT_EQ(map, conv.mapFromMessage(message)); 34 | } 35 | -------------------------------------------------------------------------------- /semantic_map_test/test/semantic_map_tests.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | int main(int argc, char **argv) { 22 | testing::InitGoogleTest(&argc, argv); 23 | srand((int)time(0)); 24 | 25 | return RUN_ALL_TESTS(); 26 | } 27 | -------------------------------------------------------------------------------- /semantic_map_to_owl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_map_to_owl) 3 | 4 | set(CMAKE_CXX_FLAGS -std=c++11) 5 | 6 | find_package( 7 | catkin 8 | REQUIRED 9 | nodelet 10 | roscpp 11 | roscpp_nodewrap 12 | semantic_map_msgs 13 | ) 14 | 15 | find_package(PkgConfig) 16 | pkg_check_modules(OWLCPP REQUIRED libowlcpp) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS 20 | include 21 | LIBRARIES 22 | semantic_map_to_owl 23 | DEPENDS 24 | nodelet 25 | roscpp 26 | roscpp_nodewrap 27 | semantic_map_msgs 28 | ) 29 | 30 | include_directories( 31 | include 32 | ${OWLCPP_INCLUDE_DIRS} 33 | ${catkin_INCLUDE_DIRS} 34 | ) 35 | 36 | add_library( 37 | semantic_map_to_owl 38 | src/MapToOwl.cpp 39 | ) 40 | 41 | target_link_libraries( 42 | semantic_map_to_owl 43 | ${OWLCPP_LIBRARIES} 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | add_executable( 48 | map_to_owl_node 49 | src/map_to_owl_node.cpp 50 | ) 51 | 52 | target_link_libraries( 53 | map_to_owl_node 54 | semantic_map_to_owl 55 | ${catkin_LIBRARIES} 56 | ) 57 | -------------------------------------------------------------------------------- /semantic_map_to_owl/config/map_to_owl.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/ros-semantic-map/69a29ec79656daa641590ec59582e8634d3741a7/semantic_map_to_owl/config/map_to_owl.yaml -------------------------------------------------------------------------------- /semantic_map_to_owl/include/semantic_map_to_owl/MapToOwl.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | /** \file MapToOwl.h 20 | * \brief Header file providing the MapToOwl class interface 21 | */ 22 | 23 | #ifndef ROS_SEMANTIC_MAP_TO_OWL_H 24 | #define ROS_SEMANTIC_MAP_TO_OWL_H 25 | 26 | #include 27 | #include 28 | 29 | namespace semantic_map { 30 | namespace owl { 31 | /** \brief Semantic map to OWL conversion node implementation 32 | */ 33 | class MapToOwl : 34 | public nodewrap::NodeImpl { 35 | public: 36 | /** \brief Default constructor 37 | */ 38 | MapToOwl(); 39 | 40 | /** \brief Destructor 41 | */ 42 | virtual ~MapToOwl(); 43 | 44 | protected: 45 | /** \brief Initialize the semantic map to OWL conversion node 46 | */ 47 | void init(); 48 | 49 | /** \brief Cleanup the semantic map to OWL conversion node 50 | */ 51 | void cleanup(); 52 | }; 53 | }; 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /semantic_map_to_owl/launch/map_to_owl_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /semantic_map_to_owl/launch/map_to_owl_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /semantic_map_to_owl/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ROS nodelet for converting semantic map descriptions to OWL format. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /semantic_map_to_owl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_map_to_owl 3 | 0.0.1 4 | 5 | ROS node for converting semantic map descriptions to OWL format. 6 | 7 | Ralf Kaestner 8 | 9 | LGPL 10 | 11 | Ralf Kaestner 12 | 13 | catkin 14 | 15 | nodelet 16 | roscpp 17 | roscpp_nodewrap 18 | semantic_map_msgs 19 | 20 | nodelet 21 | roscpp 22 | roscpp_nodewrap 23 | semantic_map_msgs 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /semantic_map_to_owl/src/MapToOwl.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2016 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include "semantic_map_to_owl/MapToOwl.h" 20 | 21 | NODEWRAP_EXPORT_CLASS(semantic_map_to_owl, semantic_map::owl::MapToOwl) 22 | 23 | namespace semantic_map { namespace owl { 24 | 25 | /*****************************************************************************/ 26 | /* Constructors and Destructor */ 27 | /*****************************************************************************/ 28 | 29 | MapToOwl::MapToOwl() { 30 | } 31 | 32 | MapToOwl::~MapToOwl() { 33 | } 34 | 35 | /*****************************************************************************/ 36 | /* Methods */ 37 | /*****************************************************************************/ 38 | 39 | void MapToOwl::init() { 40 | } 41 | 42 | void MapToOwl::cleanup() { 43 | } 44 | 45 | }} 46 | -------------------------------------------------------------------------------- /semantic_map_to_owl/src/map_to_owl_node.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Copyright (C) 2014 by Ralf Kaestner * 3 | * ralf.kaestner@gmail.com * 4 | * * 5 | * This program is free software; you can redistribute it and/or modify * 6 | * it under the terms of the Lesser GNU General Public License as published by* 7 | * the Free Software Foundation; either version 3 of the License, or * 8 | * (at your option) any later version. * 9 | * * 10 | * This program is distributed in the hope that it will be useful, * 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 13 | * Lesser GNU General Public License for more details. * 14 | * * 15 | * You should have received a copy of the Lesser GNU General Public License * 16 | * along with this program. If not, see . * 17 | ******************************************************************************/ 18 | 19 | #include 20 | 21 | #include "semantic_map_to_owl/MapToOwl.h" 22 | 23 | int main(int argc, char** argv) { 24 | ros::init(argc, argv, "semantic_map_to_owl"); 25 | 26 | nodewrap::Node node; 27 | 28 | ros::spin(); 29 | 30 | return 0; 31 | } 32 | --------------------------------------------------------------------------------