diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..eaf99b9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2024, KnowRob + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md index 79d7655..74130db 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,15 @@ # knowrob_ros -**knowrob_ros** is a ROS 1 wrapper for [KnowRob](https://github.com/knowrob/knowrob), providing ROS interfaces to integrate KnowRob's knowledge reasoning capabilities with robotic systems. This package bridges KnowRob's Prolog-based system with common ROS workflows, making it easier to perform semantic reasoning in real-world robotic tasks. +**knowrob_ros** is a ROS 1 wrapper for [KnowRob](https://github.com/knowrob/knowrob), providing ROS interfaces to integrate KnowRob's knowledge reasoning capabilities with robotic systems. + +## Key Features + +- **ROS Actions** + - `AskOne`, `AskAll`, **AskIncremental**, and `Tell` +- **Python API** + - `knowrob_ros_lib.py` for simple query/assertion via ros in python +- **Docker Support** + - Ready-to-use container for fast setup ## Installation (Local ROS Workspace) @@ -21,7 +30,7 @@ cd ~/catkin_ws/src git clone https://github.com/knowrob/knowrob.git # Clone knowrob_ros (this repo) -git clone https://github.com/your-org/knowrob_ros.git +git clone https://github.com/knowrob/knowrob_ros.git ``` ### 2. Build the workspace @@ -32,7 +41,7 @@ catkin build ``` --- -## 🐳 Installation with Docker +## Installation with Docker If you'd like to avoid setting up everything locally, you can use the provided Dockerfile to run KnowRob and knowrob_ros in an isolated container. @@ -59,9 +68,48 @@ source /catkin_ws/devel/setup.bash roslaunch knowrob_ros knowrob.launch ``` -## Example Usage +## Usage + +### ROS Interfaces + +The following ROS interfaces are available: + +| Endpoint | Type | Description | +|------------------------------------------------|--------------------------------------|-------------------------------| +| `/knowrob/askone` | `AskOneAction` | Single-result query | +| `/knowrob/askall` | `AskAllAction` | Multi-result query | +| `/knowrob/askincremental` | `AskIncrementalAction` | Start incremental query | +| `/knowrob/askincremental_next_solution` | `AskIncrementalNextSolutionAction` | Fetch next solution | +| `/knowrob/askincremental_finish` (service) | `AskIncrementalFinish.srv` | Finish incremental query | +| `/knowrob/tell` | `TellAction` | Assert triples into KB | + +All endpoints need to be called with a `ModalFrame` message. We document the `ModalFrame` message in detail below. + +### Using the Interface from Python + +Use the Python client library to integrate KnowRob queries and assertions directly into your code. Here’s a minimal example: + +```python +from knowrob_ros.knowrob_ros_lib import KnowRobRosLib, get_default_modalframe + +# Initialize ROS node and client +client = KnowRobRosLib() +client.init_node("my_python_client") + +# Single-result query +modal = get_default_modalframe() +response = client.ask_one("lpn:jealous(lpn:vincent, X)", modal) +print("AskOne status:", response.status) +print("Binding:", response.answer) + +# Shutdown when done +client.shutdown_node() +``` + +For detailed usage, full method reference, and advanced examples, see the [Python API Guide](src/knowrob_ros_lib/README.md). -### Query Interface + +### Command Line To run a query insterface on the terminal (for docker you can connect to the same container in which you launched knowrob), you should first list to the corresponding topic, on which the results are published: @@ -93,4 +141,51 @@ goal: confidence: 0.0" ``` -You should now see an answer on the first terminal. \ No newline at end of file +You should now see an answer on the first terminal. + +## Modal Frames + +The `ModalFrame` message controls **who** β€œknows” what, and **when** it’s considered true. Supply one on every call to `ask_one`, `ask_all`, `ask_incremental`, or `tell`. + +| Field | Type | Default | Description | +|-----------------------|----------|----------------------------------|-----------------------------------------------------------------------------------------------------------------------------------| +| `epistemicOperator` | `uint8` | `KNOWLEDGE (0)` | **KNOWLEDGE:** β€œit is certain that …”
**BELIEF (1):** β€œit could be that …” | +| `aboutAgentIRI` | `string` | `""` | IRI of another agent whose knowledge/belief you’re querying. Mutually exclusive with `aboutSimulationIRI`. | +| `aboutSimulationIRI` | `string` | `""` | IRI of a mental simulation knowledge graph to query. Mutually exclusive with `aboutAgentIRI`. | +| `temporalOperator` | `uint8` | `CURRENTLY (0)` | **CURRENTLY:** β€œit is now the case that …”
**ALL_PAST (1):** β€œit always was the case that …”
**SOME_PAST (2):** β€œit was the case that …” | +| `minPastTimestamp` | `float` | `UNSPECIFIED_TIMESTAMP (-1.0)` | Lower time bound (seconds since epoch). When >0, only facts true *after* this timestamp are considered. | +| `maxPastTimestamp` | `float` | `UNSPECIFIED_TIMESTAMP (-1.0)` | Upper time bound (seconds since epoch). When >0, only facts true *before* this timestamp are considered. | +| `confidence` | `float` | `0.0` | Minimum confidence threshold (0.0–1.0) for returned statements. | + +## Repository Layout + +``` +. +β”œβ”€β”€ action/ ← ROS action definitions +β”œβ”€β”€ include/ ← C++ interface headers +β”œβ”€β”€ launch/ ← Launch files +β”œβ”€β”€ msg/ ← Custom ROS messages +β”œβ”€β”€ scripts/ ← Python test scripts +β”œβ”€β”€ src/ ← Core Prolog, C++ and Python code +β”‚ └── knowrob_ros_lib/ ← Python client library +β”œβ”€β”€ srv/ ← ROS services +β”œβ”€β”€ test/ ← rostest definitions +β”œβ”€β”€ Dockerfile ← Container setup +β”œβ”€β”€ build-docker.sh ← Helper for Docker builds +β”œβ”€β”€ CMakeLists.txt +β”œβ”€β”€ package.xml +└── README.md +``` + +## Contributing + +1. Fork the repo and create a branch +2. Write or update tests in `scripts/` or `test/` +3. Follow ROS and Python style guidelines +4. Submit a pull request + +## πŸ›  Support + +- **Issues & feature requests**: [GitHub Issues](https://github.com/your-org/knowrob_ros/issues) + +(Inactive issues will be closed after 2 weeks of inactivity. Please open a new issue if you need further assistance.) diff --git a/include/marker/publisher.h b/include/marker/publisher.h deleted file mode 100644 index a0fae8f..0000000 --- a/include/marker/publisher.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef __KNOWROB_MARKER_PUBLISHER__ -#define __KNOWROB_MARKER_PUBLISHER__ - -#include -#include - -// ROS -#include -#include -#include -// SWI Prolog -#define PL_SAFE_ARG_MACROS -#include - -/** - * A marker publisher that maps Prolog terms to marker messages. - */ -class MarkerPublisher -{ -public: - MarkerPublisher(ros::NodeHandle &node); - - /** - * Publish an array of marker messages. - */ - void publish(visualization_msgs::MarkerArray &array_msg); - - /** - * Sets the current marker from a Prolog term and returns a reference - * to the marker. - */ - const visualization_msgs::Marker& setMarker(const PlTerm &term); - -protected: - ros::Publisher pub_; - visualization_msgs::Marker msg_; - std::map idMap_; - int idCounter_; - - int getID(const std::string &name); -}; - -#endif //__KNOWROB_MARKER_PUBLISHER__ diff --git a/include/tf/logger.h b/include/tf/logger.h deleted file mode 100644 index 0601395..0000000 --- a/include/tf/logger.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef __KNOWROB_TF_LOGGER__ -#define __KNOWROB_TF_LOGGER__ - -#include - -// MONGO -#include -// ROS -#include -#include -#include -// SWI Prolog -#define PL_SAFE_ARG_MACROS -#include - -#include - -/** - * A TF listener that stores messages in MongoDB. - */ -class TFLogger -{ -public: - TFLogger(ros::NodeHandle &node, - TFMemory &memory, - const std::string &topic="tf"); - ~TFLogger(); - - void set_db_name(const std::string &db_name) - { db_name_ = db_name; } - - void set_db_uri(const std::string &db_uri) - { db_uri_ = db_uri; } - - const std::string& get_db_name() - { return db_name_; } - - void set_time_threshold(double v) - { timeThreshold_ = v; } - - double get_time_threshold() const - { return timeThreshold_; } - - void set_vectorial_threshold(double v) - { vectorialThreshold_ = v; } - - double get_vectorial_threshold() const - { return vectorialThreshold_; } - - void set_angular_threshold(double v) - { angularThreshold_ = v; } - - double get_angular_threshold() const - { return angularThreshold_; } - - void store(const geometry_msgs::TransformStamped &ts); - -protected: - ros::Subscriber subscriber_; - ros::Subscriber subscriber_static_; - TFMemory &memory_; - double vectorialThreshold_; - double angularThreshold_; - double timeThreshold_; - std::string db_name_; - std::string db_uri_; - std::string topic_; - - char buf_[16]; - size_t keylen_; - - void store_document(bson_t *doc); - - bool ignoreTransform(const geometry_msgs::TransformStamped &ts); - - void callback(const tf::tfMessage::ConstPtr& msg); - - void appendTransform(bson_t *ts_doc, - const geometry_msgs::TransformStamped &ts); -}; - -#endif //__KNOWROB_TF_LOGGER__ diff --git a/include/tf/memory.h b/include/tf/memory.h deleted file mode 100644 index 4175edd..0000000 --- a/include/tf/memory.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef __KNOWROB_TF_MEMORY__ -#define __KNOWROB_TF_MEMORY__ - -#include -#include -#include -#include - -// MONGO -#include -// ROS -#include -#include -#include -// SWI Prolog -#define PL_SAFE_ARG_MACROS -#include - -/** - * A cache of most recent poses. - */ -class TFMemory -{ -public: - TFMemory(); - - /** - * True if a transform with given frame was added before. - */ - bool has_transform(const std::string &frame) const; - - /** - * Clears cached transforms and the list of managed frames. - */ - bool clear(); - - /** - * Clears cached transforms. - */ - bool clear_transforms_only(); - - /** - * Get the transform associated to a frame. - */ - const geometry_msgs::TransformStamped& get_transform(const std::string &frame, int buffer_index=0) const; - - /** - * Read a Prolog pose term into TransformStamped. - */ - void create_transform(geometry_msgs::TransformStamped *ts, const std::string &frame, const PlTerm &term, double stamp); - - /** - * Add a transform, overwriting any previous transform with same frame. - */ - void set_transform(const geometry_msgs::TransformStamped &ts); - - /** - * Add a transform, overwriting any previous transform with same frame. - */ - void set_managed_transform(const geometry_msgs::TransformStamped &ts); - - /** - * Read Prolog pose term for frame. - */ - bool get_pose_term(const std::string &frame, PlTerm *term, double *stamp); - - /** - * Add a transform, overwriting any previous transform with same frame. - */ - bool set_pose_term(const std::string &frame, const PlTerm &term, double stamp); - - /** - * True for frames which are managed by the KB. - */ - bool is_managed_frame(const std::string &frame) const; - - bool loadTF(tf::tfMessage &tf_msg, bool clear_memory); - -protected: - std::set managed_frames_[2]; - std::map transforms_[2]; - std::mutex transforms_lock_; - std::mutex names_lock_; - int buffer_index_; - - void loadTF_internal(tf::tfMessage &tf_msg, int buffer_index); -}; - -#endif //__KNOWROB_TF_MEMORY__ diff --git a/include/tf/publisher.h b/include/tf/publisher.h deleted file mode 100644 index afa20fd..0000000 --- a/include/tf/publisher.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef __KNOWROB_TF_PUBLISHER__ -#define __KNOWROB_TF_PUBLISHER__ - -#include - -// ROS -#include -#include -// SWI Prolog -#define PL_SAFE_ARG_MACROS -#include - -#include - -/** - * TF publisher for frames that are managed by the KB. - */ -class TFPublisher -{ -public: - TFPublisher(TFMemory &memory, - double frequency=10.0, - bool clear_after_publish=false); - ~TFPublisher(); - -protected: - TFMemory &memory_; - bool is_running_; - double frequency_; - bool clear_after_publish_; - std::thread thread_; - - void publishTransforms(tf2_ros::TransformBroadcaster &tf_broadcaster); - void loop(); -}; - -#endif //__KNOWROB_TF_PUBLISHER__ diff --git a/include/tf/republisher.h b/include/tf/republisher.h deleted file mode 100644 index cc211ae..0000000 --- a/include/tf/republisher.h +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef __KNOWROB_TF_REPUBLISHER__ -#define __KNOWROB_TF_REPUBLISHER__ - -#include - -// MONGO -#include -// ROS -#include -#include -#include -// SWI Prolog -#define PL_SAFE_ARG_MACROS -#include - -#include -#include -#include - -/** - * A TF publisher that publishes data stored in mongo DB. - */ -class TFRepublisher -{ -public: - TFRepublisher(double frequency=10.0); - - ~TFRepublisher(); - - void set_realtime_factor(double realtime_factor) - { realtime_factor_ = realtime_factor; } - - void set_loop(bool loop) - { loop_ = loop; } - - void set_db_name(std::string db_name) - { db_name_ = db_name; } - - void set_db_uri(const std::string &db_uri) - { db_uri_ = db_uri; } - - void set_db_collection(std::string db_collection) - { db_collection_ = db_collection; } - - TFMemory& memory() - { return memory_; } - - void set_goal(double time_min, double time_max); - - void set_now(double time); - - void set_progress(double percent); - - void clear(); - -protected: - double realtime_factor_; - double frequency_; - bool loop_; - bool is_running_; - bool reset_; - bool skip_reset_; - bool has_been_skipped_; - std::thread thread_; - std::thread tick_thread_; - - double time_min_; - double time_max_; - double time_; - - std::string db_name_; - std::string db_uri_; - std::string db_collection_; - std::shared_ptr collection_; - - mongoc_cursor_t *cursor_; - geometry_msgs::TransformStamped ts_; - bool has_next_; - bool has_new_goal_; - - TFMemory memory_; - TFPublisher publisher_; - - void loop(); - void tick_loop(); - void create_cursor(double start_time); - void reset_cursor(); - void advance_cursor(); - void read_transform(const bson_t *doc); - void set_initial_poses(double unix_time); -}; - -#endif //__KNOWROB_TF_REPUBLISHER__ diff --git a/launch/apartment.launch b/launch/apartment.launch deleted file mode 100755 index 31fb429..0000000 --- a/launch/apartment.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/launch/map.launch b/launch/map.launch deleted file mode 100755 index d8367a2..0000000 --- a/launch/map.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/launch/openease-test.launch b/launch/openease-test.launch deleted file mode 100755 index dbe0d6f..0000000 --- a/launch/openease-test.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/launch/openease.launch b/launch/openease.launch deleted file mode 100755 index 66cf411..0000000 --- a/launch/openease.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/parser_vis.launch b/launch/parser_vis.launch deleted file mode 100755 index ad2976a..0000000 --- a/launch/parser_vis.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/rosinstall/knowrob-base.rosinstall b/rosinstall/knowrob-base.rosinstall deleted file mode 100755 index 199e951..0000000 --- a/rosinstall/knowrob-base.rosinstall +++ /dev/null @@ -1,3 +0,0 @@ -- git: {local-name: knowrob, uri: 'https://github.com/knowrob/knowrob.git', version: master } -- git: {local-name: rosprolog, uri: 'https://github.com/knowrob/rosprolog.git', version: master } -- git: {local-name: iai_common_msgs, uri: 'https://github.com/code-iai/iai_common_msgs.git', version: master } diff --git a/src/ROS.pl b/src/ROS.pl deleted file mode 100755 index 5b4e589..0000000 --- a/src/ROS.pl +++ /dev/null @@ -1,353 +0,0 @@ -:- module('knowrob/model/ROSOWL', []). - -%:- module('knowrob/model/ROSOWL', -% [ -% ros_type_path/2, -% ros_primitive_type/2, -% ros_array_type/2, -% ros_service/3, -% rosowl_service_call/3, -% ros_request_encode/2, -% ros_response_decode/2, -% ros_message_conversion/3, -% create_ros_request/5 -% ]). -% -%:- use_module(library('semweb/rdf_db')). -%:- use_module(library('semweb/rdfs')). -%:- use_module(library('semweb/owl')). -%:- use_module(library('http/json')). -% -%:- use_module(library('knowrob/comp/rdf_data'), [kb_rdf_pl/3]). -% -%:- rdf_meta ros_type_path(r,?), -% ros_primitive_type(?,r), -% ros_service(r, ?, ?), -% ros_request_encode(r, ?), -% ros_response_decode(+, -), -% rosowl_service_call(r, r, r), -% create_ros_request(r,t,t,r,r). -% -%:- multifile ros_message_conversion/3. -% -%%% ros_type_path(+MessageType,?TypePath) is det. -%% -%ros_type_path(MessageType,TypePath) :- -% kb_type_of(MessageType,ros:'MessageType'), -% kb_triple(MessageType,ros:hasTypePath,TypePath),!. -%ros_type_path(PrimitiveType,TypePath) :- -% kb_type_of(PrimitiveType,ros:'PrimitiveType'), -% rdf_split_url(_, TypePath, PrimitiveType),!. -%ros_type_path(ArrayType, ArrayType_atom) :- -% kb_type_of(ArrayType,ros:'ArrayType'), -% once(( -% kb_triple(ArrayType,dul:hasPart,X), -% ros_type_path(X,T))), -% term_to_atom(array(T), ArrayType_atom),!. -% -%%% ros_primitive_type(?ROS_type, ?RDF_type) is det. -%% -%% A mapping between ROS and RDF types. -%% -%ros_primitive_type('bool', 'http://www.w3.org/2001/XMLSchema#boolean'). -%ros_primitive_type('float32', 'http://www.w3.org/2001/XMLSchema#float'). -%ros_primitive_type('float64', 'http://www.w3.org/2001/XMLSchema#double'). -%ros_primitive_type('int8', 'http://www.w3.org/2001/XMLSchema#byte'). -%ros_primitive_type('int16', 'http://www.w3.org/2001/XMLSchema#short'). -%ros_primitive_type('int32', 'http://www.w3.org/2001/XMLSchema#int'). -%ros_primitive_type('int64', 'http://www.w3.org/2001/XMLSchema#long'). -%ros_primitive_type('uint8', 'http://www.w3.org/2001/XMLSchema#unsignedByte'). -%ros_primitive_type('uint16', 'http://www.w3.org/2001/XMLSchema#unsignedShort'). -%ros_primitive_type('uint32', 'http://www.w3.org/2001/XMLSchema#unsignedInt'). -%ros_primitive_type('uint64', 'http://www.w3.org/2001/XMLSchema#unsignedLong'). -%ros_primitive_type('string', 'http://www.w3.org/2001/XMLSchema#string'). -%% TODO support duration and time -%%ros_primitive_type('duration',_). -%%ros_primitive_type('time',xsd:dateTime). -% -%ros_array_type('bool', 'http://www.ease-crc.org/ont/EASE.owl#array_boolean'). -%ros_array_type('float32', 'http://www.ease-crc.org/ont/EASE.owl#array_float'). -%ros_array_type('float64', 'http://www.ease-crc.org/ont/EASE.owl#array_double'). -%ros_array_type('int8', 'http://www.ease-crc.org/ont/EASE.owl#array_int'). -%ros_array_type('int16', 'http://www.ease-crc.org/ont/EASE.owl#array_int'). -%ros_array_type('int32', 'http://www.ease-crc.org/ont/EASE.owl#array_int'). -%ros_array_type('int64', 'http://www.ease-crc.org/ont/EASE.owl#array_int'). -%ros_array_type('uint8', 'http://www.ease-crc.org/ont/EASE.owl#array_uint'). -%ros_array_type('uint16', 'http://www.ease-crc.org/ont/EASE.owl#array_uint'). -%ros_array_type('uint32', 'http://www.ease-crc.org/ont/EASE.owl#array_uint'). -%ros_array_type('uint64', 'http://www.ease-crc.org/ont/EASE.owl#array_uint'). -%ros_array_type('string', 'http://www.ease-crc.org/ont/EASE.owl#array_string'). -% -%%% ros_service(+Service,?Name,?Path) is det. -%% -%ros_service(Service,Name,Path) :- -% kb_triple(Service,ros:concretelyImplements,ServiceInterface), -% kb_triple(Service,ros:hasServiceName,Name), -% kb_triple(ServiceInterface,ros:hasTypePath,Path). -% -%%% ros_message_slot_type(+Msg,?SlotName,?SlotType) is det. -%% -%ros_message_slot_type(Msg,SlotName,SlotType) :- -% kb_triple(Msg,dul:realizes,Msg_Type), -% kb_triple(Msg_Type,dul:hasPart,SlotType), -% kb_triple(SlotType,ros:hasSlotName,SlotName),!. -% -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -%% % % % % % % % % % % call a service -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -% -%%% rosowl_service_call(+Service, +Request, +Response) is semidet. -%% -%rosowl_service_call(Service, Request, Response) :- -% ros_service(Service, ServiceName, ServicePath), -% ( ros_request_encode(Request, Request_json) ; -% throw(ros_error(ros:'SerializationFailure')) ), -% ( ros_json_service_call(_{ -% service_path: ServicePath, -% service_name: ServiceName, -% json_data: Request_json -% }, Response_json) ; -% throw(ros_error(ros:'ServiceNodeUnreachable')) ), -% ( ground(Response_json) ; % no response from service -% throw(ros_error(ros:'ServiceNodeUnreachable')) ), -% ( ros_response_decode(Response_json, Response) ; -% throw(ros_error(ros:'SerializationFailure')) ). -% -%%% ros_request_encode(+Request, -Request_json) is det. -%% -%ros_request_encode(Request, Request_json) :- -% ros_entity_to_prolog(Request, Request_dict),!, -% %%%%%%%%% -% %%%%% encode as JSON dict -% %%%%%%%%% -% with_output_to(atom(Request_json), -% json_write_dict(current_output, Request_dict) -% ). -% -%%% ros_response_decode(+Response_json, +Response) is det. -%% -%ros_response_decode(Response_json, Response) :- -% %%%%%%%%% -% %%%%% Encode response as list of key-value pairs -% %%%%%%%%% -% atom_to_chars(Response_json,Response_chars), -% open_chars_stream(Response_chars, Response_Stream), -% json_read_dict(Response_Stream, Response_dict), -% dict_pairs(Response_dict,_,Response_pairs), -% %%%%%%%%% -% %%%%% Encode response as list of pairs -% %%%%%%%%% -% forall(( -% member(SName-SValue, Response_pairs), -% ros_message_slot_type(Response, SName, DataSlot)),( -% %%%%%%%%% -% %%%%% Create symbolic representation for response field -% %%%%%%%%% -% once(( -% kb_triple(DataSlot,dul:hasPart,SlotType), -% ros_type_path(SlotType,SType) -% )), -% owl_create_ros_entity(SType,SValue,Slot), -% kb_assert(Response,dul:hasPart,Slot), -% kb_assert(Slot,dul:realizes,DataSlot) -% )). -% -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -%% % % % % % % % % % % geometry_msgs/Transform -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -% -%ros_message_conversion('geometry_msgs/Transform', -% get_transform_dict, -% get_transform_region). -% -%get_transform_dict(Message,_{ -% translation: ['geometry_msgs/Vector3', -% _{x:['float64',Tx], y:['float64',Ty], z:['float64',Tz]}], -% rotation: ['geometry_msgs/Quaternion', -% _{x:['float64',Qx], y:['float64',Qy], z:['float64',Qz], w:['float64',Qw]}] -% }) :- -% kb_triple(Message,dul:hasRegion,Region), -% kb_triple(Region, knowrob:translation, [Tx,Ty,Tz]), -% kb_triple(Region, knowrob:quaternion, [Qx,Qy,Qz,Qw]). -% -%get_transform_region(_{ -% translation: _{x:Tx, y:Ty, z:Tz}, -% rotation: _{x:Qx, y:Qy, z:Qz, w:Qw} -% }, Region) :- -% kb_create(knowrob:'Pose',Region), -% kb_assert(Region, knowrob:translation, [Tx,Ty,Tz]), -% kb_assert(Region, knowrob:quaternion, [Qx,Qy,Qz,Qw]). -% -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -%% % % % % % % % % % % OWL to ROS message dict -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -% -%ros_message_to_prolog(_, Msg, Msg_dict) :- -% kb_triple(Msg,dul:realizes,Msg_Type), -% findall(SName-[SType,SValue], ( -% %%%%%%%%% -% %%%%% For each slot of the message type -% %%%%%%%%% -% kb_triple(Msg_Type,dul:hasPart,DataSlot), -% once(( -% kb_triple(DataSlot, ros:hasSlotName, SName), -% kb_triple(DataSlot,dul:hasPart,SlotType), -% ros_type_path(SlotType,SType) -% )), -% %%%%%%%%% -% %%%%% Infer the value of the slot -% %%%%%%%%% -% once(( -% kb_triple(Msg, dul:hasPart, Filler), -% kb_triple(Filler, dul:realizes, DataSlot), -% ros_entity_to_prolog(Filler, SValue) -% )) -% ), Pairs), -% % create a dict 'msg{key1:[type1,value1],...}' -% dict_pairs(Msg_dict,msg,Pairs). -% -%ros_entity_to_prolog(Msg, Msg_dict) :- -% kb_type_of(Msg,ros:'Message'),!, -% kb_triple(Msg,dul:realizes,Msg_Type), -% kb_triple(Msg_Type,ros:hasTypePath,TypePath), -% once((( -% % first check if message is represented as region -% ros_message_conversion(TypePath, GetDictGoal, _), -% call(GetDictGoal, Msg, Msg_dict)) ; -% % elese traverse message parts -% ros_message_to_prolog(TypePath, Msg, Msg_dict))). -% -%ros_entity_to_prolog(PrimitiveValue_owl, PrimitiveValue_pl) :- -% ( kb_type_of(PrimitiveValue_owl,ros:'PrimitiveValue') ; -% kb_type_of(PrimitiveValue_owl,ros:'PrimitiveArray') ),!, -% kb_triple(PrimitiveValue_owl,dul:hasRegion,Region), -% (( rdf_has(Region,dul:hasRegionDataValue,X), -% kb_rdf_pl(dul:hasRegionDataValue,X,PrimitiveValue_pl) -% ); PrimitiveValue_pl = Region -% ), !. -% -%ros_entity_to_prolog(Array, List) :- -% kb_type_of(Array,ros:'Array'),!, -% kb_triple(Array, dul:concretelyExpresses, Collection), -% ros_entity_to_prolog(Collection, List). -% -%ros_entity_to_prolog(Collection, List) :- -% kb_type_of(Collection,dul:'Collection'),!, -% ( kb_triple(Collection, soma:firstMember, First) -> -% owl_sequence(First, List) ; -% findall(M, kb_triple(Collection,dul:hasMember, M), Xs) ), -% findall(X, ( -% member(Iri,Xs), -% ros_entity_to_prolog(Iri,X)), -% List). -% -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -%% % % % % % % % % % % ROS message dict to OWL -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % -% -%owl_create_ros_entity(Msg_TypePath,Msg_dict,Msg_owl) :- -% is_dict(Msg_dict),!, -% kb_create(ros:'Message',Msg_owl), -% ( kb_triple(Msg_Type,ros:hasTypePath,Msg_TypePath) -> -% kb_assert(Msg_owl,dul:realizes,Msg_Type) ; true ), -% once(( -% ros_message_conversion(Msg_TypePath, _, GetRegionGoal), -% call(GetRegionGoal,Msg_dict,Region_owl), -% kb_assert(Msg_owl,dul:hasRegion,Region_owl) -% );( -% dict_pairs(Msg_dict,_,Msg_pairs), -% forall(member(SName-[SType,SValue], Msg_pairs),( -% owl_create_ros_entity(SType,SValue,Val_owl), -% kb_assert(Val_owl,soma:hasNameString,SName), -% kb_assert(Msg_owl,dul:hasPart,Val_owl) -% )) -% )). -% -%owl_create_ros_entity(Type,Value,PrimitiveValue) :- -% ros_primitive_type(Type, XSDType),!, -% kb_create(dul:'Region',Region), -% ( atom(Value) -> Atom = Value ; term_to_atom(Value,Atom) ), -% kb_assert(Region,dul:hasRegionDataValue, -% literal(type(XSDType,Atom))), -% kb_create(ros:'PrimitiveValue',PrimitiveValue), -% kb_assert(PrimitiveValue,dul:hasRegion,Region). -% -%owl_create_ros_entity(Array_type,Val_list,PrimitiveArray) :- -% is_list(Val_list), -% term_to_atom(array(Type),Array_type), -% ros_primitive_type(Type, _),!, -% ros_array_type(Type, ArrayType), -% % list to atom -% findall(A, (member(X,Val_list), term_to_atom(X,A)), Atoms), -% atomic_list_concat(Atoms, ' ', ArrayData), -% % create symbols -% kb_create(ros:'PrimitiveArray',PrimitiveArray), -% kb_create(dul:'Region',Region), -% kb_assert(Region,dul:hasRegionDataValue, -% literal(type(ArrayType,ArrayData))), -% kb_assert(PrimitiveArray,dul:hasRegion,Region). -% -%owl_create_ros_entity(ArrayTypePath,Val_list,MessageArray) :- -% is_list(Val_list), -% term_to_atom(array(Type),ArrayTypePath), -% ros_type_path(ArrayType,ArrayTypePath), -% findall(Val_owl, ( -% member(X,Val_list), -% owl_create_ros_entity(Type,X,Val_owl)), -% Msgs), -% kb_create(dul:'Collection',Collection), -% forall( member(X,Msgs), kb_assert(Collection,dul:hasMember,X) ), -% kb_create(ros:'MessageArray',MessageArray), -% kb_assert(MessageArray, dul:realizes, ArrayType), -% kb_assert(MessageArray, dul:concretelyExpresses, Collection). -% -%%% create_ros_request(Action,InputDict,ReqType,Request) -%% -%% -%create_ros_request(Action,InputDict,ActionDict,ReqType,Request) :- -% %%%%%%%%% -% %%%%% Create request message -% kb_create(ros:'Message',Request), -% kb_assert(Request,dul:realizes,ReqType), -% kb_assert(Action,ros:hasRequest,Request), -% %%%%%%%%% -% %%%%% Set request slots -% forall(kb_triple(ReqType,dul:hasPart,DataSlot), once(( -% % for each data slot, find participant of the -% % action that is classified by the same parameter or role -% get_dict(Concept,InputDict,Filler), -% get_dict(Concept,ActionDict,DataSlot), -% %kb_triple(DataSlot,dul:hasPart,SlotType), -% create_ros_message_slot(DataSlot,Filler,Slot), -% kb_assert(Request,dul:hasPart,Slot), -% kb_assert(Slot,dul:realizes,DataSlot) -% ))). -% -%%% -%create_ros_message_slot(SlotType, Region, Slot) :- -% rdfs_individual_of(SlotType,ros:'PrimitiveSlot'),!, -% rdfs_individual_of(Region,dul:'Region'), -% kb_create(ros:'PrimitiveValue',Slot), -% kb_assert(Slot, dul:hasRegion, Region). -% -%create_ros_message_slot(SlotType, Region, Slot) :- -% rdfs_individual_of(SlotType,ros:'ArraySlot'), -% rdfs_individual_of(Region,dul:'Region'),!, -% kb_create(ros:'PrimitiveArray',Slot), -% kb_assert(Slot, dul:hasRegion, Region). -% -%create_ros_message_slot(SlotType, Array, Array) :- -% rdfs_individual_of(SlotType,ros:'ArraySlot'),!, -% rdfs_individual_of(Array,ros:'Array'). -% -%create_ros_message_slot(SlotType, Region, Message) :- -% rdfs_individual_of(SlotType,ros:'MessageSlot'), -% rdfs_individual_of(Region,dul:'Region'), -% kb_triple(SlotType,dul:hasPart,MessageType),!, -% kb_create(ros:'Message',Message), -% kb_assert(Message,dul:realizes,MessageType), -% kb_assert(Message,dul:hasRegion,Region). -% -%create_ros_message_slot(SlotType, Message, Message) :- -% rdfs_individual_of(SlotType,ros:'MessageSlot'),!, -% rdfs_individual_of(Message,ros:'Message'). diff --git a/src/__init__.pl b/src/__init__.pl deleted file mode 100755 index 7d87a60..0000000 --- a/src/__init__.pl +++ /dev/null @@ -1,14 +0,0 @@ - -% make sure library path is expanded -:- register_ros_package(knowrob). - -% register ROS packages to resolve IRI prefixes to local paths -:- ros_package_iri(knowrob, 'http://www.ontologydesignpatterns.org/ont/dul'). -:- ros_package_iri(knowrob, 'http://www.ease-crc.org/ont'). -:- ros_package_iri(knowrob, 'http://knowrob.org/kb'). -:- ros_package_iri(knowrob, 'http://www.w3.org/2002/07'). %/owl.rdf - -% load init files in sub-directories -:- use_directory('urdf'). -:- use_directory('tf'). -:- use_directory('marker'). diff --git a/src/knowrob_ros_lib/README.md b/src/knowrob_ros_lib/README.md new file mode 100644 index 0000000..4eab11a --- /dev/null +++ b/src/knowrob_ros_lib/README.md @@ -0,0 +1,196 @@ +# knowrob_ros Python Client Library + +`knowrob_ros_lib.py` provides a lightweight interface to KnowRob's ROS action and service endpoints. + +## Quickstart + +A minimal end-to-end example that: +1. Initializes ROS and the client +2. Performs an AskOne query +3. Shuts down cleanly + +```python +from knowrob_ros.knowrob_ros_lib import KnowRobRosLib, get_default_modalframe + +# 1) Initialize +client = KnowRobRosLib() +client.init_node("quickstart_client") + +# 2) Query for "lpn:jealous(lpn:vincent, X)" +modal = get_default_modalframe() +res = client.ask_one("lpn:jealous(lpn:vincent, X)", modal) + +print(f"Status: {res.status}\nBinding: {res.answer}") + +# 3) Shutdown +client.shutdown_node() +``` + +For more detailed examples (AskAll, incremental queries, assertions), see the [Python API Guide](../src/knowrob_ros_lib/README.md). + +--- + +## API Reference + +### `class KnowRobRosLib` + +High-level client exposing all KnowRob ROS endpoints: + +| Method | Description | +|------------------------------------------------|------------------------------------------------------------------| +| `init_node(name: str)` | Initialize ROS node, action clients, and service proxies. | +| `shutdown_node()` | Shutdown ROS node and cancel any pending goals. | +| `ask_one(query: str, frame: ModalFrame, lang)` | Single-result query; returns `AskOneResult`. | +| `ask_all(query: str, frame: ModalFrame, lang)` | Multi-result query; returns `AskAllResult`. | +| `ask_incremental(query: str, frame: ModalFrame)` | Start incremental query; returns `AskIncrementalResult`. | +| `next_solution(query_id: int)` | Get next solution for incremental query; returns `AskIncrementalNextSolutionResult`. | +| `finish_incremental(query_id: int)` | Finish incremental query; returns `bool` success indicator. | +| `tell(triples: list[Triple], frame: ModalFrame)` | Assert a list of RDF triples; returns `TellResult`. | + + +## Tutorial + +This tutorial walks through common workflows step-by-step, from basic queries to advanced incremental patterns. + +### 1. Basic Single-Result Query + +```python +# Import client and modal frame utility +from knowrob_ros.knowrob_ros_lib import KnowRobRosLib, get_default_modalframe, +binding = graph_answer_to_dict + + +# Create the client instance +client = KnowRobRosLib() # Instantiate the KnowRob ROS client + +# Initialize the ROS node named 'tutorial_client' +client.init_node("tutorial_client") # Must be called before any ROS actions + +# Obtain a default modal frame for contextual queries +modal = get_default_modalframe() # Sets up epistemic & temporal defaults + +# Execute a single-result query (AskOne) +result = client.ask_one( + "lpn:jealous(lpn:vincent, X)", # Prolog query string + modal # ModalFrame argument +) + +# Check the query status +if result.status == AskOneResult.TRUE: # TRUE indicates a successful match + # Convert the raw GraphAnswerMessage to a Python dict + binding = graph_answer_to_dict(result.answer) + # Print the bound value for X + print("Jealous of:", binding['X']) +else: + # Handle cases where no result is found or an error occurred + print("No result or query failed") + +# Clean shutdown of the ROS client +client.shutdown_node() # Gracefully closes connections and cancels goals +``` + +### 2. Retrieving All Matches + +```python +from knowrob_ros.knowrob_ros_lib import KnowRobRosLib, get_default_modalframe, graph_answers_to_list + +# 1) Initialize client and node +client = KnowRobRosLib() # Instantiate client +client.init_node("tutorial_client") # Initialize ROS node +modal = get_default_modalframe() # Default modal frame + +# 2) Execute a multi-result query (AskAll) +res_all = client.ask_all( + "childOf(X, lpn:vincent)", # Query to find all children of Vincent + modal # ModalFrame argument +) + +# 3) Convert list of GraphAnswerMessage to Python list of dicts +bindings = graph_answers_to_list(res_all.answers) # [{ 'X': ... }, ...] + +# 4) Iterate and print each binding +for b in bindings: + print(b) # Print each result dictionary + +# 5) Shutdown +client.shutdown_node() # Graceful shutdown +``` + +### 3. Incremental Queries + +```python +from knowrob_ros.knowrob_ros_lib import ( + KnowRobRosLib, + get_default_modalframe, + graph_answer_to_dict +) + +# 1) Initialize +client = KnowRobRosLib() # Instantiate client +client.init_node("tutorial_client") # Initialize ROS node +modal = get_default_modalframe() # Default modal frame + +# 2) Start incremental query (AskIncremental) +inc = client.ask_incremental( + "relatedTo(X, Y)", # Query string for relation pairs + modal # ModalFrame +) +# The server returns a queryId to fetch solutions one by one +query_id = inc.queryId # Unique identifier for this incremental session + +# 3) Loop until no more solutions +while True: + sol = client.next_solution(query_id) # Fetch next solution + if sol.finished: # Check if iteration is complete + break + # Convert and print the answer + binding = graph_answer_to_dict(sol.answer) # Extract variable bindings + print(binding) # Print the dict, e.g. { 'X': ..., 'Y': ... } + +# 4) Release resources on the server +success = client.finish_incremental(query_id) # Invoke service to finish +print("Finished incremental:", success) # True if clean + +# 5) Shutdown +client.shutdown_node() # Graceful shutdown +``` + +### 4. Asserting New Knowledge + +```python +from knowrob_ros.knowrob_ros_lib import ( + KnowRobRosLib, + get_default_modalframe, + TripleQueryBuilder, + graph_answers_to_list +) +from knowrob_ros.msg import TellResult + +# 1) Initialize client and modal frame +client = KnowRobRosLib() # Instantiate client +client.init_node("tutorial_client") # Initialize ROS node +modal = get_default_modalframe() # Default modal frame + +# 2) Build RDF-like triples to assert +builder = TripleQueryBuilder() # Helper to create Triple messages +builder.add("alice", "marriedTo", "frank") # Define a new fact +triples = builder.get_triples() # Retrieve list of Triple messages + +# 3) Send triples to KnowRob (Tell action) +tell_res = client.tell(triples, modal) # Perform assertion +if tell_res.status == TellResult.TRUE: # Check for success + print("Successfully asserted marriage relationship.") +else: + print("Assertion failed.") # Handle failure case + +# 4) Verify assertion via AskAll query +check = client.ask_all( + "marriedTo(alice, X)", # Query newly asserted relationship + modal # ModalFrame +) +bindings = graph_answers_to_list(check.answers) # Convert results +print("Bindings:", bindings) # Expect X = frank + +# 5) Shutdown +client.shutdown_node() # Graceful shutdown +``` diff --git a/src/marker/README.md b/src/marker/README.md deleted file mode 100755 index 8cc0d15..0000000 --- a/src/marker/README.md +++ /dev/null @@ -1,17 +0,0 @@ -\page ros_marker Marker visualization - -The ROS-based tool RViz defines a commonly used message format -for 3D scene visualization supported by KnowRob. -Knowledge about objects is used to generate marker visualization -messages and tools such as RViz will then be able to visiualize -a 3D scene accordingly. -In particular, knowledge about certain qualities of objects is exploited such -as the color or shape of the object. - -For example, this code can be used to generate visualization markers for the PR2 robot in initial pose -using "map" as root frame of the world: - - load_owl('package://knowrob/owl/robots/PR2.owl'), - urdf_load('http://knowrob.org/kb/PR2.owl#PR2_0', 'package://knowrob/urdf/pr2_for_unit_tests.urdf', [load_rdf]), - urdf_set_pose_to_origin('http://knowrob.org/kb/PR2.owl#PR2_0',map), - marker:republish. diff --git a/src/marker/__init__.pl b/src/marker/__init__.pl deleted file mode 100755 index f2e3fee..0000000 --- a/src/marker/__init__.pl +++ /dev/null @@ -1,2 +0,0 @@ - -:- use_module('marker'). diff --git a/src/marker/marker.pl b/src/marker/marker.pl deleted file mode 100644 index b35dede..0000000 --- a/src/marker/marker.pl +++ /dev/null @@ -1,266 +0,0 @@ -:- module(marker, - [ show_marker/2, - show_marker/3, - show_markers/1, - hide_marker/1, - marker_type/2, - marker_action/2 - ]). - -:- use_module(library(settings)). -:- use_module(library('scope'), - [ current_scope/1 ]). -:- use_module('object_marker'). - -:- use_foreign_library('libmarker_knowrob.so'). - -:- multifile marker_factory/3. - -% define some settings -%:- setting(auto, boolean, true, -% 'Toggle whether marker messages are generated automatically when an object changes.'). -%:- setting(reference_frame, atom, '', -% 'The reference frame in which the position of markers is published.'). - -%% -%:- message_queue_create(_,[alias(ros_marker_queue)]). - -%% marker_action(?ActionTerm,?ActionID) is det. -% -% Maps action term to type id. -% -% @param ActionTerm marker action term -% @param ActionID marker action id -% -marker_action(add, 0). -marker_action(modify, 1). % NOTE: deprecated -marker_action(delete, 2). -marker_action(deleteall, 3). - -%% marker_type(?TypeTerm,?TypeID) is det. -% -% Maps type term to type id. -% -% @param TypeTerm marker type term -% @param TypeID marker type id -% -marker_type(arrow, 0). -marker_type(cube, 1). -marker_type(sphere, 2). -marker_type(cylinder, 3). -marker_type(line_strip, 4). -marker_type(line_list, 5). -marker_type(cube_list, 6). -marker_type(sphere_list, 7). -marker_type(points, 8). -marker_type(text_view_facing, 9). -marker_type(mesh_resource, 10). -marker_type(triangle_list, 11). - -%% show_marker(+MarkerID,+MarkerTerm) is semidet. -% -% Same as show_marker/2 with empty options list. -% -% @param MarkerID marker id -% @param MarkerTerm marker term -% -show_marker(MarkerID, MarkerTerm) :- - show_marker(MarkerID, MarkerTerm, []). - -%% show_marker(+MarkerID,+MarkerTerm,+Options) is semidet. -% -% Add a visualization marker. -% -% @param MarkerID marker id -% @param MarkerTerm marker term -% @param Options marker options -% -show_marker(_MarkerID, _MarkerTerm, _Options) :- - writeln('WARNING show_marker called'), -% thread_send_message( -% ros_marker_queue, -% marker(add, MarkerID, MarkerTerm, Options) -% ). - fail. - - -%% show_marker(+TimePoint) is semidet. -% -% Republish all markers of a given timepoint -% -% @param Timepoint The given timepoint -% -show_markers(Timepoint) :- - %time_scope(=<(Timepoint), >=(Timepoint), Scope), - %forall( - % kb_call(is_physical_object(PO)), - % show_marker(PO, PO, [scope(Scope)]) - %). - findall(Msg, - ( object_marker(_Obj,ID,Data), - marker_message_new(ID,Data,Msg) - ), - MessageList - ), - ( MessageList=[] -> true - ; marker_array_publish(MessageList) - ). - -%% -% Republish all object markers. -% -republish :- - get_time(Now), - show_markers(Now). - - -%% hide_marker(+MarkerID) is det. -% -% Remove a visualization marker. -% -% @param MarkerID marker id -% -hide_marker(_MarkerID) :- - writeln('WARNING hide_marker called'), - fail. -% thread_send_message( -% ros_marker_queue, -% marker(delete, MarkerID, _, []) -% ). - -%% -marker_message_new(MarkerID,Data, - [Action,MarkerID,Type,Pose,Scale,Color,Mesh,Text]) :- - !, - option(type(TypeTerm),Data), - option(pose(Pose),Data), - option(scale(Scale),Data,[1,1,1]), - option(color(Color),Data,[1,1,1,1]), - option(mesh(Mesh),Data,''), - option(text(Text),Data,''), - %% - marker_action(add,Action), - marker_type(TypeTerm,Type). - -%% -% Get marker data as Prolog list: -% [Action,ID,Type,Pose,Scale,Color,Mesh,Text] -% -marker_message(marker(add,ID,Term,Parameters),Msg) :- - !, - %% get marker data - get_marker_scope(Parameters,Scope), - marker_message1(Term,[[],Scope]->_,ID->ID0,Data0), - %% overwrite parameters - merge_options(Parameters,Data0,Data), - %% - marker_message_new(ID0,Data,Msg). - -marker_message(marker(modify,ID,Term,Opts),Msg) :- - marker_message(marker(add,ID,Term,Opts),Msg), - !. - -marker_message(marker(delete_all,_,_,_),[Action]) :- - marker_action(delete_all,Action), - !. - -marker_message(marker(delete,ID,_,_),[Action,ID]) :- - marker_action(delete,Action), - !. - -%% -marker_message1(Object,Scope,_->ID,MarkerData) :- - atom(Object), - !, - object_marker(Object,Scope,ID,MarkerData). - -marker_message1(MarkerData,_,ID->ID,MarkerData) :- - is_list(MarkerData), - !. - -marker_message1(MarkerTerm,Scope,ID->ID,MarkerData) :- - compound(MarkerTerm), - marker_factory(MarkerTerm,Scope,MarkerData), - !. - -%% -get_marker_scope(Options,Scope) :- - option(scope(Scope),Options), - !. - -get_marker_scope(Options,Scope) :- - option(time(Stamp),Options), - time_scope(=<(Stamp), >=(Stamp), Scope), - !. - -get_marker_scope(_,Scope) :- - current_scope(Scope), - !. - -%% -% Get all queued markers. -% -%marker_pull_all([Head|Rest],Opts) :- -% thread_get_message( -% ros_marker_queue, -% Head, -% Opts), -% !, -% marker_pull_all(Rest,[timeout(0)]). -% -%marker_pull_all([], _). - -%% -% Delete redundant markers. -% -%marker_to_set([marker(_,ID,_,_)|Xs],Ys) :- -% % ignore in case another marker with same ID is in Xs -% member(marker(_,ID,_,_),Xs), -% !, -% marker_to_set(Xs,Ys). -% -%marker_to_set([X|Xs],[X|Ys]) :- -% !, -% marker_to_set(Xs,Ys). -% -%marker_to_set([],[]). - -%% -% Loop and publish queued markers. -% -%marker_loop :- -% repeat, -% %% -% log_info(loop0), -% marker_pull_all(MarkerTerms,[]), -% log_info(loop1), -% get_time(T0), -% marker_to_set(MarkerTerms,MarkerTerms0), -% findall(MarkerMessage, -% ( member(MarkerTerm,MarkerTerms0), -% marker_message(MarkerTerm,MarkerMessage) -% ), -% MessageList), -% ( MessageList=[] -> true -% ; marker_array_publish(MessageList) -% ), -% get_time(T1), -% length(MarkerTerms0,Count), -% Took is T1 - T0, -% log_info(looped(Took,Count)), -% %% -% fail. - -%% -% Start thread when this file is consulted. -% -%:- thread_create(marker_loop,_). - -%% -% notify_hook is called whenever an object changes. -% -%notify:notify_hook(object_changed(Obj)) :- -% ( setting(marker:auto,true) -% -> show_marker(Obj, Obj, []) -% ; true -% ). diff --git a/src/marker/object_marker.pl b/src/marker/object_marker.pl deleted file mode 100644 index 29a1822..0000000 --- a/src/marker/object_marker.pl +++ /dev/null @@ -1,107 +0,0 @@ -:- module(object_marker, - [ object_marker/3 - ]). - -:- use_module(library('semweb/rdf_db'), - [ rdf_split_url/3 ]). -:- use_module(library('model/SOMA'), - [ object_shape/5 ]). - -%% object_marker -% -% Maps an object entity to its marker parameters. -% -% @param Obj object IRI -% @param MarkerData marker parameters -% -object_marker(Obj,_,_) :- - atom(Obj), - is_urdf_joint(Obj), !, - fail. - -object_marker(Obj,MarkerID,MarkerData) :- - catch( - object_marker0(Obj,MarkerID,MarkerData), - Exc, - (log_error(Exc),fail) - ). - -object_marker0(Obj,MarkerID, - [ pose(Origin) | MarkerData ]) :- - object_shape(Obj,MarkerID,Shape,Origin0,Material), - object_origin(Origin0,Origin), - object_marker1(Shape,Material,MarkerData). - -object_marker1( - mesh(MeshPath,Scale), Material, - [ type(mesh_resource), - mesh(MeshPath), - scale(Scale), - color(RGBA) - ]) :- - ( file_name_extension(_, stl, MeshPath) - -> material_rgba(Material,RGBA) - ; RGBA=[0,0,0,0] - ). - -object_marker1( - box(X,Y,Z), Material, - [ type(cube), - scale([X,Y,Z]), - color(RGBA) - ]) :- - material_rgba(Material,RGBA). - -object_marker1( - sphere(Radius), Material, - [ type(sphere), - scale([Radius,Radius,Radius]), - color(RGBA) - ]) :- - material_rgba(Material,RGBA). - -object_marker1( - cylinder(Radius,Length), Material, - [ type(cylinder), - scale([Radius,Radius,Length]), - color(RGBA) - ]) :- - material_rgba(Material,RGBA). - -%% -object_origin([Frame,Pos0,Rot0],[Frame,Pos1,Rot1]) :- - ( is_list(Pos0) -> Pos1=Pos0 - ; atom(Pos0) -> parse_number_list(Pos0,Pos1) - ; var(Pos0) -> Pos1 = [0,0,0] - ; Pos1=Pos0 - ), - ( is_list(Rot0) -> Rot1=Rot0 - ; atom(Rot0) -> parse_number_list(Rot0,Rot1) - ; var(Rot0) -> Rot1 = [0,0,0,1] - ; Rot1=Rot0 - ). - -% -parse_number_list(Atom,NumberList) :- - catch(term_to_atom(NumberList,Atom), _, fail),!. -parse_number_list(Atom,NumberList) :- - atomic_list_concat(Elems, ' ', Atom), - maplist(atom_number, Elems, NumberList). - -%% -material_rgba(material(Material),[R,G,B,A]) :- - member(rgba([R,G,B,A]),Material), - ground([R,G,B,A]), - !. - -material_rgba(material(Material),[R,G,B,A]) :- - member(rgba([R,G,B],A),Material), - ground([R,G,B,A]), - !. - -material_rgba(material(Material),[R,G,B,1]) :- - member(rgb([R,G,B]),Material), - ground([R,G,B]), - !. - -material_rgba(_,[1,1,1,1]). diff --git a/src/marker/publisher.cpp b/src/marker/publisher.cpp deleted file mode 100644 index fa9b0f6..0000000 --- a/src/marker/publisher.cpp +++ /dev/null @@ -1,105 +0,0 @@ -#include - -MarkerPublisher::MarkerPublisher(ros::NodeHandle &node) : - pub_(node.advertise("visualization_marker_array", 5)), - idCounter_(0) -{ - msg_.ns = "belief_state"; - msg_.frame_locked = true; - msg_.mesh_use_embedded_materials = true; - msg_.lifetime = ros::Duration(); -} - -void MarkerPublisher::publish(visualization_msgs::MarkerArray &array_msg) -{ - pub_.publish(array_msg); -} - -int MarkerPublisher::getID(const std::string &name) -{ - std::map::iterator it; - it = idMap_.find(name); - if(it == idMap_.end()) { - idMap_[name] = idCounter_; - return idCounter_++; - } - else { - return it->second; - } -} - -const visualization_msgs::Marker& MarkerPublisher::setMarker(const PlTerm &data_term) -{ - // data_term=[Action,ID,Type,Pose,Scale,Color,Mesh,Text] - PlTail l0(data_term); - PlTerm e0, e1, e2; - - l0.next(e0); msg_.action = (int)e0; - if(msg_.action == visualization_msgs::Marker::DELETEALL) - return msg_; - - l0.next(e0); msg_.id = getID(std::string((char*)e0)); - if(msg_.action == visualization_msgs::Marker::DELETE) - return msg_; - - l0.next(e0); msg_.type = (int)e0; - - // read pose term [frame,position,rotation] - l0.next(e0); { - PlTail l1(e0); - l1.next(e1); msg_.header.frame_id = (char*)e1; - l1.next(e1); { - PlTail l2(e1); - l2.next(e2); msg_.pose.position.x = (double)e2; - l2.next(e2); msg_.pose.position.y = (double)e2; - l2.next(e2); msg_.pose.position.z = (double)e2; - } - l1.next(e1); { - PlTail l2(e1); - l2.next(e2); msg_.pose.orientation.x = (double)e2; - l2.next(e2); msg_.pose.orientation.y = (double)e2; - l2.next(e2); msg_.pose.orientation.z = (double)e2; - l2.next(e2); msg_.pose.orientation.w = (double)e2; - } - } - - // read scale vector - l0.next(e0); { - PlTail l1(e0); - l1.next(e1); msg_.scale.x = (double)e1; - l1.next(e1); msg_.scale.y = (double)e1; - l1.next(e1); msg_.scale.z = (double)e1; - } - - - // read color data - l0.next(e0); { - PlTail l1(e0); - l1.next(e1); msg_.color.r = (double)e1; - l1.next(e1); msg_.color.g = (double)e1; - l1.next(e1); msg_.color.b = (double)e1; - l1.next(e1); msg_.color.a = (double)e1; - } - - l0.next(e0); msg_.mesh_resource = (char*)e0; - l0.next(e0); msg_.text = (char*)e0; - - return msg_; -} - -static ros::NodeHandle node; -static MarkerPublisher pub(node); - -PREDICATE(marker_array_publish, 1) -{ - visualization_msgs::MarkerArray msg; - // populate array - PlTail list(PL_A1); - PlTerm member; - while(list.next(member)) { - msg.markers.push_back(pub.setMarker(member)); - } - // - pub.publish(msg); - return true; -} diff --git a/src/tf/README.md b/src/tf/README.md deleted file mode 100755 index 58eec9f..0000000 --- a/src/tf/README.md +++ /dev/null @@ -1,63 +0,0 @@ -\page ros_tf TF integration - -KnowRob stores the most recent transforms in local memory. -This is also the source of data for a TF publisher running in this module. -There is a distinction between frames that are thought to be "managed" by KnowRob -and those that are not. -KnowRob will only publish managed frames on the TF topic. -This is needed to avoid clashes between other TF publishing nodes such as the -robot_state_publisher and the one in KnowRob. - - -## TF logging - -This module includes a logger for TF messages stored in a mongo database. -The logger keeps track of previous transforms, and only writes into the database -if some threshold was exceeded. -TF messages are not stored in array format as this would complicate indexing the data. -The logger can be toggled on or off through a setting: - - setting(tf:use_logger, false). - - -## TF fluent - -The fastest way to query persisted TF data is through the fluent predicate tf/4: - -| Predicate | Arguments | -| --- | --- | -| tf/4 | +Frame, -ReferenceFrame, -Position, -Rotation | - -The predicate compiles into a mongo DB query, and can be potentially -combined with other goals that can be computed server-side by mongo DB. -As it is defined as a fluent predicate, it will allways yield the most up-to-date -TF record of the child frame. -It also supports querying past fluent values in case they have been persisted in the -database. -There exists another variant with two arguments where the second argument is a list -of parent frame, position, and rotation. -Note that this will only work if TF data is stored in mongo DB, e.g. by using the TF logger -in this module. - -It is also possible to write into the database by referring to tf/4 in an -assertion, as in: - - kb_call(assert(tf(myframe, map, [0,0,0], [0,0,0,1]))). - -However, this will _not_ update the internal TF memory KnowRob maintains -(see tf_mem_set_pose/3). -Further note that tf/4 does not allow to request a pose in some specific reference frame, -for that purpose is_at/2 can be used. - - -## TF computable - -In many cases, a pose needs to be transformed into some specific reference frame in which -it was not persisted in the database. -A computable predicate is_at/2 is defined to support exactly this: to compute the pose of -a frame wrt. some reference frame. -Within this module, is_at/2 is implemented by the predicate tf_get_pose/4. -For example: - - kb_call(is_at(ns:'MyObject', [target_frame, Position, Rotation]))). - diff --git a/src/tf/__init__.pl b/src/tf/__init__.pl deleted file mode 100755 index 01dca3e..0000000 --- a/src/tf/__init__.pl +++ /dev/null @@ -1,3 +0,0 @@ - -:- use_module('tf'). -:- use_module('tf_mongo'). diff --git a/src/tf/logger.cpp b/src/tf/logger.cpp deleted file mode 100644 index 5537e3e..0000000 --- a/src/tf/logger.cpp +++ /dev/null @@ -1,151 +0,0 @@ -#include - -#include -#include - -TFLogger::TFLogger( - ros::NodeHandle &node, - TFMemory &memory, - const std::string &topic) : - memory_(memory), - timeThreshold_(-1.0), - vectorialThreshold_(0.001), - angularThreshold_(0.001), - db_name_("roslog"), - topic_(topic), - subscriber_static_(node.subscribe("tf_static", 1000, &TFLogger::callback, this)), - subscriber_(node.subscribe(topic, 1000, &TFLogger::callback, this)) -{ -} - -TFLogger::~TFLogger() -{ -} - -void TFLogger::store_document(bson_t *doc) -{ - bson_error_t err; - std::shared_ptr collection_ = MongoInterface::get().connect( - db_uri_.c_str(), db_name_.c_str(),topic_.c_str()); - if(!mongoc_collection_insert( - (*collection_)(),MONGOC_INSERT_NONE,doc,NULL,&err)) - { - ROS_WARN("[TFLogger] insert failed: %s.", err.message); - } -} - -void TFLogger::store(const geometry_msgs::TransformStamped &ts) -{ - bson_t *doc = bson_new(); - appendTransform(doc, ts); - BSON_APPEND_DATE_TIME(doc, "__recorded", time(NULL) * 1000); - BSON_APPEND_UTF8(doc, "__topic", topic_.c_str()); - store_document(doc); - bson_destroy(doc); -} - -void TFLogger::callback(const tf::tfMessage::ConstPtr& msg) -{ - std::vector::const_iterator it; - for (it = msg->transforms.begin(); it != msg->transforms.end(); ++it) - { - const geometry_msgs::TransformStamped &ts = *it; - if(!ignoreTransform(ts)) { - store(ts); - // NOTE: IMPORTANT: we assign *ts* to its frame in memory - // such that we can check for the pose difference in - // ignoreTransform. - // This is only needed for the frames that are not - // managed by knowrob (e.g. robot frames handled by robot - // state publisher). - if(!memory_.is_managed_frame(ts.child_frame_id)) { - memory_.set_transform(ts); - } - } - } -} - -bool TFLogger::ignoreTransform(const geometry_msgs::TransformStamped &ts0) -{ - const std::string &child = ts0.child_frame_id; - if(!memory_.has_transform(child)) { - // it's a new frame - return false; - } - if(memory_.is_managed_frame(child)) { - // managed frames are asserted into DB back-end directly - return true; - } - const geometry_msgs::TransformStamped &ts1 = memory_.get_transform(child); - // do not ignore in case parent frame has changed - const std::string &parent0 = ts0.header.frame_id; - const std::string &parent1 = ts1.header.frame_id; - if(parent0.compare(parent1)!=0) { - return false; - } - // tests whether temporal distance exceeds threshold - if(timeThreshold_>0.0) { - double timeDistance = ( - (ts0.header.stamp.sec * 1000.0 + ts0.header.stamp.nsec / 1000000.0) - - (ts1.header.stamp.sec * 1000.0 + ts1.header.stamp.nsec / 1000000.0)) / 1000.0; - if(timeDistance<0 || timeDistance > timeThreshold_) { - return false; - } - } - // tests whether vectorial distance exceeds threshold - const geometry_msgs::Vector3 &pos0 = ts0.transform.translation; - const geometry_msgs::Vector3 &pos1 = ts1.transform.translation; - double vectorialDistance = sqrt( - ((pos0.x - pos1.x) * (pos0.x - pos1.x)) + - ((pos0.y - pos1.y) * (pos0.y - pos1.y)) + - ((pos0.z - pos1.z) * (pos0.z - pos1.z))); - if(vectorialDistance > vectorialThreshold_) { - return false; - } - // tests whether angular distance exceeds threshold - const geometry_msgs::Quaternion &rot0 = ts0.transform.rotation; - const geometry_msgs::Quaternion &rot1 = ts1.transform.rotation; - tf::Quaternion q1(rot0.x, rot0.y, rot0.z, rot0.w); - tf::Quaternion q2(rot1.x, rot1.y, rot1.z, rot1.w); - float angularDistance = 2.0 * fabs(q1.angle(q2)); - if(angularDistance > angularThreshold_) { - return false; - } - return true; -} - -void TFLogger::appendTransform(bson_t *ts_doc, const geometry_msgs::TransformStamped &ts) -{ - bson_t child, child2; - // append header - BSON_APPEND_DOCUMENT_BEGIN(ts_doc, "header", &child); { - unsigned long long time = (unsigned long long)( - ts.header.stamp.sec * 1000.0 + ts.header.stamp.nsec / 1000000.0); - // - BSON_APPEND_INT32(&child, "seq", ts.header.seq); - BSON_APPEND_DATE_TIME(&child, "stamp", time); - BSON_APPEND_UTF8(&child, "frame_id", ts.header.frame_id.c_str()); - bson_append_document_end(ts_doc, &child); - } - // append child frame - BSON_APPEND_UTF8(ts_doc, "child_frame_id", ts.child_frame_id.c_str()); - // append transform - BSON_APPEND_DOCUMENT_BEGIN(ts_doc, "transform", &child); { - // append translation - BSON_APPEND_DOCUMENT_BEGIN(&child, "translation", &child2); { - BSON_APPEND_DOUBLE(&child2, "x", ts.transform.translation.x); - BSON_APPEND_DOUBLE(&child2, "y", ts.transform.translation.y); - BSON_APPEND_DOUBLE(&child2, "z", ts.transform.translation.z); - bson_append_document_end(&child, &child2); - } - // append rotation - BSON_APPEND_DOCUMENT_BEGIN(&child, "rotation", &child2); { - BSON_APPEND_DOUBLE(&child2, "x", ts.transform.rotation.x); - BSON_APPEND_DOUBLE(&child2, "y", ts.transform.rotation.y); - BSON_APPEND_DOUBLE(&child2, "z", ts.transform.rotation.z); - BSON_APPEND_DOUBLE(&child2, "w", ts.transform.rotation.w); - bson_append_document_end(&child, &child2); - } - bson_append_document_end(ts_doc, &child); - } -} diff --git a/src/tf/memory.cpp b/src/tf/memory.cpp deleted file mode 100644 index 7bad75a..0000000 --- a/src/tf/memory.cpp +++ /dev/null @@ -1,217 +0,0 @@ -#include - -static inline double get_stamp(const geometry_msgs::TransformStamped &ts) -{ - unsigned long long time = (unsigned long long)( - ts.header.stamp.sec * 1000.0 + ts.header.stamp.nsec / 1000000.0); - return (double)(time/1000.0); -} - -static geometry_msgs::TransformStamped dummy; - -//#define SEND_UNKNOWN_FAR_AWAY -#ifdef SEND_UNKNOWN_FAR_AWAY -static geometry_msgs::TransformStamped far_away; -#endif - -TFMemory::TFMemory() : - buffer_index_(0) -{ -#ifdef SEND_UNKNOWN_FAR_AWAY - far_away.transform.translation.x = 99999.9; - far_away.transform.translation.y = 99999.9; - far_away.transform.translation.z = 99999.9; - far_away.transform.rotation.x = 0.0; - far_away.transform.rotation.y = 0.0; - far_away.transform.rotation.z = 0.0; - far_away.transform.rotation.w = 1.0; - far_away.header.frame_id = "map"; -#endif -} - -bool TFMemory::has_transform(const std::string &frame) const -{ - return transforms_[buffer_index_].find(frame) != transforms_[buffer_index_].end(); -} - -bool TFMemory::is_managed_frame(const std::string &frame) const -{ - return managed_frames_[buffer_index_].find(frame) != managed_frames_[buffer_index_].end(); -} - -bool TFMemory::clear() -{ - std::lock_guard guard1(transforms_lock_); - std::lock_guard guard2(names_lock_); - transforms_[buffer_index_].clear(); - managed_frames_[buffer_index_].clear(); - return true; -} - -bool TFMemory::clear_transforms_only() -{ - std::lock_guard guard1(transforms_lock_); - transforms_[buffer_index_].clear(); - return true; -} - -const geometry_msgs::TransformStamped& TFMemory::get_transform( - const std::string &frame, int buffer_index) const -{ - const std::map::const_iterator - &needle = transforms_[buffer_index].find(frame); - if(needle != transforms_[buffer_index].end()) { - return needle->second; - } - else { - return dummy; - } -} - -void TFMemory::set_transform(const geometry_msgs::TransformStamped &ts) -{ - std::lock_guard guard(transforms_lock_); - transforms_[buffer_index_][ts.child_frame_id] = ts; -} - -void TFMemory::set_managed_transform(const geometry_msgs::TransformStamped &ts) -{ - std::lock_guard guard1(transforms_lock_); - std::lock_guard guard2(names_lock_); - managed_frames_[buffer_index_].insert(ts.child_frame_id); - transforms_[buffer_index_][ts.child_frame_id] = ts; -} - -bool TFMemory::loadTF(tf::tfMessage &tf_msg, bool clear_memory) -{ - if(managed_frames_[buffer_index_].empty()) { - return true; - } - - if(clear_memory) { - int pong = buffer_index_; - // ping-pong. pong buffer can then be used without lock. - // and other threads start writing into ping buffer. - buffer_index_ = (pong==0 ? 1 : 0); - // load transforms without locking - loadTF_internal(tf_msg,pong); - // clear the pong buffer - managed_frames_[pong].clear(); - transforms_[pong].clear(); - } - else { - // load transforms while locking *managed_frames_*. - // it is ok though if transforms_ is written to. - std::lock_guard guard(names_lock_); - loadTF_internal(tf_msg,buffer_index_); - } - return true; -} - -void TFMemory::loadTF_internal(tf::tfMessage &tf_msg, int buffer_index) -{ - const ros::Time& time = ros::Time::now(); - // loop over all frames - for(std::set::const_iterator - it=managed_frames_[buffer_index].begin(); - it!=managed_frames_[buffer_index].end(); ++it) - { - const std::string &name = *it; - std::map::iterator - needle = transforms_[buffer_index].find(name); - if(needle != transforms_[buffer_index].end()) { - geometry_msgs::TransformStamped &tf_transform = needle->second; - tf_transform.header.stamp = time; - tf_msg.transforms.push_back(tf_transform); - } -#ifdef SEND_UNKNOWN_FAR_AWAY - else { - far_away.header.stamp = time; - far_away.child_frame_id = name; - tf_msg.transforms.push_back(far_away); - } -#endif - } -} - -bool TFMemory::get_pose_term(const std::string &frame, PlTerm *term, double *stamp) -{ - if(!has_transform(frame)) return false; - const geometry_msgs::TransformStamped &ts = get_transform(frame); - - PlTail pose_list(*term); { - pose_list.append(ts.header.frame_id.c_str()); - // - PlTerm pos_term; - PlTail pos_list(pos_term); { - pos_list.append(ts.transform.translation.x); - pos_list.append(ts.transform.translation.y); - pos_list.append(ts.transform.translation.z); - pos_list.close(); - } - pose_list.append(pos_term); - // - PlTerm rot_term; - PlTail rot_list(rot_term); { - rot_list.append(ts.transform.rotation.x); - rot_list.append(ts.transform.rotation.y); - rot_list.append(ts.transform.rotation.z); - rot_list.append(ts.transform.rotation.w); - rot_list.close(); - } - pose_list.append(rot_term); - pose_list.close(); - } - // get unix timestamp - *stamp = get_stamp(ts); - - return true; -} - -bool TFMemory::set_pose_term(const std::string &frame, const PlTerm &term, double stamp) -{ - const geometry_msgs::TransformStamped &ts_old = get_transform(frame); - // make sure the pose is more recent then the one stored - if(stamp<0.0) { - // force setting pose if stamp<0.0 - stamp = 0.0; - } - else if(get_stamp(ts_old)>stamp) { - return false; - } - // - geometry_msgs::TransformStamped ts = ts_old; - create_transform(&ts,frame,term,stamp); - set_managed_transform(ts); - return true; -} - -void TFMemory::create_transform( - geometry_msgs::TransformStamped *ts, - const std::string &frame, - const PlTerm &term, - double stamp) -{ - PlTail tail(term); PlTerm e,j; - // frame - ts->child_frame_id = frame; - // header - tail.next(e); - ts->header.frame_id = std::string((char*)e); - unsigned long long time_ms = ((unsigned long long)(stamp*1000.0)); - ts->header.stamp.sec = time_ms / 1000; - ts->header.stamp.nsec = (time_ms % 1000) * 1000 * 1000; - // translation - tail.next(e); - PlTail pos_list(e); - pos_list.next(j); ts->transform.translation.x =(double)j; - pos_list.next(j); ts->transform.translation.y =(double)j; - pos_list.next(j); ts->transform.translation.z =(double)j; - // rotation - tail.next(e); - PlTail rot_list(e); - rot_list.next(j); ts->transform.rotation.x =(double)j; - rot_list.next(j); ts->transform.rotation.y =(double)j; - rot_list.next(j); ts->transform.rotation.z =(double)j; - rot_list.next(j); ts->transform.rotation.w =(double)j; -} diff --git a/src/tf/publisher.cpp b/src/tf/publisher.cpp deleted file mode 100644 index 2613c78..0000000 --- a/src/tf/publisher.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include -#include - -// TODO: handle static object transforms (e.g. for features, srdl components also have static transforms relative to base link) - -TFPublisher::TFPublisher(TFMemory &memory, double frequency, bool clear_after_publish) : - memory_(memory), - is_running_(true), - clear_after_publish_(clear_after_publish), - frequency_(frequency), - thread_(&TFPublisher::loop, this) -{ -} - -TFPublisher::~TFPublisher() -{ - is_running_ = false; - thread_.join(); -} - -void TFPublisher::loop() -{ - tf2_ros::TransformBroadcaster tf_broadcaster; - ros::Rate r(frequency_); - while(ros::ok()) { - publishTransforms(tf_broadcaster); - r.sleep(); - if(!is_running_) break; - } -} - -void TFPublisher::publishTransforms(tf2_ros::TransformBroadcaster &tf_broadcaster) -{ - tf::tfMessage tf_msg; - memory_.loadTF(tf_msg, clear_after_publish_); - tf_broadcaster.sendTransform(tf_msg.transforms); -} diff --git a/src/tf/republisher.cpp b/src/tf/republisher.cpp deleted file mode 100644 index aae9029..0000000 --- a/src/tf/republisher.cpp +++ /dev/null @@ -1,358 +0,0 @@ -#include -#include - -#define CLEAR_MEMORY_AFTER_PUBLISH 0 - -TFRepublisher::TFRepublisher(double frequency) : - realtime_factor_(1.0), - frequency_(frequency), - loop_(true), - has_next_(false), - has_new_goal_(false), - is_running_(true), - reset_(false), - has_been_skipped_(false), - skip_reset_(false), - db_name_("neems"), - db_collection_("tf"), - collection_(NULL), - time_min_(0.0), - time_max_(0.0), - time_(0.0), - cursor_(NULL), - memory_(), - publisher_(memory_,frequency,CLEAR_MEMORY_AFTER_PUBLISH), - thread_(&TFRepublisher::loop, this), - tick_thread_(&TFRepublisher::tick_loop, this) -{ -} - -TFRepublisher::~TFRepublisher() -{ - is_running_ = false; - thread_.join(); - tick_thread_.join(); -} - -void TFRepublisher::clear() -{ - has_next_ = false; - time_min_ = 0.0; - time_max_ = 0.0; - time_ = 0.0; - memory_.clear(); -} - -void TFRepublisher::tick_loop() -{ - ros::NodeHandle node; - ros::Rate r(frequency_); - ros::Publisher tick(node.advertise("republisher_tick",1)); - std_msgs::Float64 time_msg; - double last_t = ros::Time::now().toSec(); - - while(ros::ok()) { - double this_t = ros::Time::now().toSec(); - - if(time_max_ > 0.0 ) { - double dt = this_t - last_t; - double next_time = time_ + dt*realtime_factor_; - // set new time value - if(next_time > time_max_) { - time_ = time_min_; - reset_ = true; - } - else if(next_time < time_min_) { - time_ = time_min_; - reset_ = true; - } - else { - time_ = next_time; - } - // publish time value - time_msg.data = time_; - tick.publish(time_msg); - } - - last_t = this_t; - // sleep to achieve rate of 10Hz - r.sleep(); - if(!is_running_) break; - } -} - -void TFRepublisher::loop() -{ - ros::Rate r(frequency_); - while(ros::ok()) { - if(time_>0.0) { - advance_cursor(); - } - r.sleep(); - if(!is_running_) break; - } -} - -void TFRepublisher::set_goal(double time_min, double time_max) -{ - time_min_ = time_min; - time_max_ = time_max; - time_ = time_min_; - has_next_ = false; - has_new_goal_ = true; -} - -void TFRepublisher::set_progress(double percent) -{ - time_ = time_min_ + percent*(time_max_ - time_min_); - has_been_skipped_ = true; -} - -void TFRepublisher::set_now(double time) -{ - time_ = time_min_; - has_been_skipped_ = true; -} - -void TFRepublisher::create_cursor(double start_time) -{ - // ascending order - bson_t *opts = BCON_NEW( - "sort", "{", "header.stamp", BCON_INT32 (1), "}" - ); - // filter documents outside of time interval - bson_t *filter = BCON_NEW( - "header.stamp", "{", - "$gt", BCON_DATE_TIME((unsigned long long)(1000.0*start_time)), - "$lt", BCON_DATE_TIME((unsigned long long)(1000.0*time_max_)), - "}" - ); - // get the cursor - if(cursor_!=NULL) { - mongoc_cursor_destroy(cursor_); - } - - collection_ = MongoInterface::get().connect( - db_uri_.c_str(), db_name_.c_str(),db_collection_.c_str()); - collection_->appendSession(opts); - cursor_ = mongoc_collection_find_with_opts( - (*collection_)(), filter, opts, NULL /* read_prefs */ ); - // cleanup - if(filter) { - bson_destroy(filter); - } - if(opts) { - bson_destroy(opts); - } -} - -void TFRepublisher::set_initial_poses(double unix_time) -{ - unsigned long long mng_time = (unsigned long long)(1000.0*unix_time); - bson_t *append_opts = bson_new(); - // lookup latest transform of each frame before given time, if any - bson_t *pipeline = BCON_NEW ("pipeline", "[", - "{", "$group", "{", "_id", "{", "child_frame_id", BCON_UTF8("$child_frame_id"), "}", "}", "}", - "{", "$lookup", "{", - "from", BCON_UTF8(db_collection_.c_str()), - "as", BCON_UTF8("tf"), - "let", "{", "frame", BCON_UTF8("$_id.child_frame_id"), "}", - "pipeline", "[", - "{", "$match", "{", "$expr", "{", "$and", "[", - "{", "$eq", "[", BCON_UTF8("$child_frame_id"), BCON_UTF8("$$frame"), "]", "}", - "{", "$lt", "[", BCON_UTF8("$header.stamp"), BCON_DATE_TIME(mng_time), "]", "}", - "]", "}", "}", "}", - "{", "$sort", "{", - "child_frame_id", BCON_INT32(1), - "header.stamp", BCON_INT32(-1), - "}", "}", - "{", "$limit", BCON_INT32(1), "}", - "]", - "}", "}", - "{", "$unwind", BCON_UTF8("$tf"), "}", - "{", "$replaceRoot", "{", "newRoot", BCON_UTF8("$tf"), "}", "}", - "]"); - // create the cursor - collection_ = MongoInterface::get().connect( - db_uri_.c_str(), db_name_.c_str(),db_collection_.c_str()); - collection_->appendSession(append_opts); - mongoc_cursor_t *cursor = mongoc_collection_aggregate( - (*collection_)(), MONGOC_QUERY_NONE, pipeline, NULL, NULL); - memory_.clear_transforms_only(); - if(cursor!=NULL) { - const bson_t *doc; - // iterate the cursor and assign poses in TF memory - while(mongoc_cursor_next(cursor,&doc)) { - read_transform(doc); - memory_.set_transform(ts_); - } - mongoc_cursor_destroy(cursor); - } - // cleanup - if(pipeline) { - bson_destroy(pipeline); - } - if(append_opts) { - bson_destroy(append_opts); - } -} - -void TFRepublisher::reset_cursor() -{ - if(cursor_!=NULL) { - mongoc_cursor_t *clone = mongoc_cursor_clone(cursor_); - mongoc_cursor_destroy(cursor_); - cursor_ = clone; - } -} - -void TFRepublisher::advance_cursor() -{ - double this_time = time_; - if(has_new_goal_) { - has_new_goal_ = false; - has_next_ = false; - set_initial_poses(time_min_); - create_cursor(time_min_); - } - else if(has_been_skipped_) { - has_been_skipped_ = false; - has_next_ = false; - // special reset mode because cursor needs to be recreated with proper start time - skip_reset_ = true; - // load initial poses - set_initial_poses(this_time); - // create cursor with documents from this_time to time_max_ - create_cursor(this_time); - } - if(reset_) { - reset_ = false; - has_next_ = false; - // load initial poses to avoid problems with objects sticking at the position - // where they were at the end of the loop. - set_initial_poses(time_min_); - if(skip_reset_) { - skip_reset_ = false; - create_cursor(time_min_); - } - else { - reset_cursor(); - } - } - // - while(1) { - // check if the cursor has an error. - // if this is the case, reset next loop. - bson_error_t cursor_error; - if (mongoc_cursor_error (cursor_, &cursor_error)) { - ROS_ERROR("[TFRepublisher] mongo cursor error: %s. Resetting..", cursor_error.message); - reset_ = true; - break; - } - - if(has_next_) { - double t_next = (ts_.header.stamp.sec * 1000.0 + - ts_.header.stamp.nsec / 1000000.0) / 1000.0; - if(t_next > this_time) { - // the next transform is too far in the future - break; - } - // push the next transform -#if CLEAR_MEMORY_AFTER_PUBLISH - memory_.set_managed_transform(ts_); -#else - memory_.set_transform(ts_); -#endif - } - // read the next transform - const bson_t *doc; - if(cursor_!=NULL && mongoc_cursor_next(cursor_,&doc)) { - read_transform(doc); - has_next_ = true; - } - else { - has_next_ = false; - break; - } - } -} - -void TFRepublisher::read_transform(const bson_t *doc) -{ - bson_iter_t iter; - if(!bson_iter_init(&iter,doc)) { - return; - } - - while(bson_iter_next(&iter)) { - const char *key = bson_iter_key(&iter); - - if(strcmp("child_frame_id",key)==0) { - ts_.child_frame_id = std::string(bson_iter_utf8(&iter,NULL)); - } - - else if(strcmp("header",key)==0) { - bson_iter_t header_iter; - bson_iter_recurse(&iter, &header_iter); - while(bson_iter_next(&header_iter)) { - const char *header_key = bson_iter_key(&header_iter); - if(strcmp("seq",header_key)==0) { - ts_.header.seq = bson_iter_int32(&header_iter); - } - else if(strcmp("frame_id",header_key)==0) { - ts_.header.frame_id = std::string(bson_iter_utf8(&header_iter,NULL)); - } - else if(strcmp("stamp",header_key)==0) { - int64_t msec_since_epoch = bson_iter_date_time(&header_iter); - ts_.header.stamp.sec = msec_since_epoch / 1000; - ts_.header.stamp.nsec = (msec_since_epoch % 1000) * 1000 * 1000; - } - } - } - - else if(strcmp("transform",key)==0) { - bson_iter_t transform_iter; - bson_iter_recurse(&iter, &transform_iter); - while(bson_iter_next(&transform_iter)) { - const char *transform_key = bson_iter_key(&transform_iter); - // - bson_iter_t iter1; - bson_iter_recurse(&transform_iter, &iter1); - - if(strcmp("translation",transform_key)==0) { - while(bson_iter_next(&iter1)) { - const char *key1 = bson_iter_key(&iter1); - if(strcmp("x",key1)==0) { - ts_.transform.translation.x = bson_iter_double(&iter1); - } - else if(strcmp("y",key1)==0) { - ts_.transform.translation.y = bson_iter_double(&iter1); - } - else if(strcmp("z",key1)==0) { - ts_.transform.translation.z = bson_iter_double(&iter1); - } - } - } - - else if(strcmp("rotation",transform_key)==0) { - while(bson_iter_next(&iter1)) { - const char *key1 = bson_iter_key(&iter1); - if(strcmp("x",key1)==0) { - ts_.transform.rotation.x = bson_iter_double(&iter1); - } - else if(strcmp("y",key1)==0) { - ts_.transform.rotation.y = bson_iter_double(&iter1); - } - else if(strcmp("z",key1)==0) { - ts_.transform.rotation.z = bson_iter_double(&iter1); - } - else if(strcmp("w",key1)==0) { - ts_.transform.rotation.w = bson_iter_double(&iter1); - } - } - } - } - } - } -} - diff --git a/src/tf/tf.cpp b/src/tf/tf.cpp deleted file mode 100644 index df04e81..0000000 --- a/src/tf/tf.cpp +++ /dev/null @@ -1,168 +0,0 @@ - -#include -#include -#include -#include - -static ros::NodeHandle node; -static TFMemory memory; -static TFPublisher pub(memory); -static TFLogger *tf_logger=NULL; - -// TF logger parameter -double vectorial_threshold=0.001; -double angular_threshold=0.001; -double time_threshold=-1.0; -std::string logger_db_name="roslog"; - -TFRepublisher& get_republisher() { - static TFRepublisher republisher; - return republisher; -} - -// tf_republish_set_goal(DBName,CollectionName,Time0,Time1) -PREDICATE(tf_republish_set_goal, 4) { - std::string db_name((char*)PL_A1); - std::string coll_name((char*)PL_A2); - double time_min = (double)PL_A3; - double time_max = (double)PL_A4; - get_republisher().set_db_uri(db_name); - get_republisher().set_db_collection(coll_name); - get_republisher().set_goal(time_min,time_max); - return true; -} - -PREDICATE(tf_republish_set_time, 1) { - double time = (double)PL_A1; - get_republisher().set_now(time); - return true; -} - -PREDICATE(tf_republish_set_progress, 1) { - double percent = (double)PL_A1; - get_republisher().set_progress(percent); - return true; -} - -PREDICATE(tf_republish_clear, 0) { - get_republisher().clear(); - return true; -} - -// tf_republish_set_loop(RealtimeFactor) -PREDICATE(tf_republish_set_loop, 1) { - get_republisher().set_loop((int)PL_A1); - return true; -} - -// tf_republish_set_realtime_factor(RealtimeFactor) -PREDICATE(tf_republish_set_realtime_factor, 1) { - double realtime_factor = (double)PL_A1; - get_republisher().set_realtime_factor(realtime_factor); - return true; -} - -// tf_logger_enable -PREDICATE(tf_logger_enable, 0) { - if(tf_logger) { - delete tf_logger; - } - - tf_logger = new TFLogger(node,memory); - tf_logger->set_db_name(logger_db_name); - tf_logger->set_time_threshold(time_threshold); - tf_logger->set_vectorial_threshold(vectorial_threshold); - tf_logger->set_angular_threshold(angular_threshold); - return true; -} - -// tf_logger_disable -PREDICATE(tf_logger_disable, 0) { - if(tf_logger) { - delete tf_logger; - tf_logger = NULL; - } - return true; -} - -// tf_logger_set_db_name(DBName) -PREDICATE(tf_logger_set_db_name, 1) { - std::string db_name((char*)PL_A1); - logger_db_name = db_name; - return true; -} - -// -PREDICATE(tf_logger_set_time_threshold, 1) { - time_threshold = (double)PL_A1; - return true; -} -PREDICATE(tf_logger_set_vectorial_threshold, 1) { - vectorial_threshold = (double)PL_A1; - return true; -} -PREDICATE(tf_logger_set_angular_threshold, 1) { - angular_threshold = (double)PL_A1; - return true; -} - -// -PREDICATE(tf_logger_get_time_threshold, 1) { - PL_A1 = time_threshold; - return true; -} -PREDICATE(tf_logger_get_vectorial_threshold, 1) { - PL_A1 = vectorial_threshold; - return true; -} -PREDICATE(tf_logger_get_angular_threshold, 1) { - PL_A1 = angular_threshold; - return true; -} - - -// Clear the tf memory to remove cached transforms -PREDICATE(tf_mem_clear, 0) { - memory.clear(); - return true; -} - -// tf_mem_set_pose(ObjFrame,PoseData,Since) -PREDICATE(tf_mem_set_pose, 3) { - std::string frame((char*)PL_A1); - double stamp = (double)PL_A3; - memory.set_pose_term(frame,PL_A2,stamp); - return true; -} - -// tf_republish_set_pose(ObjFrame,PoseData) -PREDICATE(tf_republish_set_pose, 2) { - std::string frame((char*)PL_A1); - get_republisher().memory().set_pose_term(frame,PL_A2,-1.0); - return true; -} - -// tf_mem_get_pose(ObjFrame,PoseData,Since) -PREDICATE(tf_mem_get_pose, 3) { - std::string frame((char*)PL_A1); - double stamp; - PlTerm pose_term; - if(memory.get_pose_term(frame,&pose_term,&stamp)) { - PL_A2 = pose_term; - PL_A3 = stamp; - return true; - } - return false; -} - -// tf_mng_store(ObjFrame,PoseData,Since) -PREDICATE(tf_mng_store, 3) { - std::string frame((char*)PL_A1); - double stamp = (double)PL_A3; - geometry_msgs::TransformStamped ts; - memory.create_transform(&ts,frame,PL_A2,stamp); - if(tf_logger) { - tf_logger->store(ts); - } - return true; -} diff --git a/src/tf/tf.pl b/src/tf/tf.pl deleted file mode 100755 index fdf9723..0000000 --- a/src/tf/tf.pl +++ /dev/null @@ -1,278 +0,0 @@ -:- module(tf, - [ tf_set_pose/3, - tf_get_pose/4, - tf_mem_set_pose/3, - tf_mem_get_pose/3, - tf_mem_clear/0, - tf_republish_set_pose/2, - tf_republish_set_goal/2, - tf_republish_set_time/1, - tf_republish_set_progress/1, - tf_republish_set_loop/1, - tf_republish_set_realtime_factor/1, - tf_republish_clear/0, - tf_logger_enable/0, - tf_logger_disable/0 - ]). - -:- use_foreign_library('libtf_knowrob.so'). - -:- use_module(library(settings)). -:- use_module(library('semweb/rdf_db'), - [ rdf_split_url/3 ]). -:- use_module(library('utility/algebra'), - [ transform_between/3, transform_multiply/3 ]). -:- use_module(library('scope'), - [ scope_intersect/3, - subscope_of/2, - time_scope/3, - time_scope_data/2, - current_scope/1 - ]). -:- use_module(library('lang/computable'), - [ add_computable_predicate/2 ]). -:- use_module('tf_mongo', - [ tf_mng_lookup/6, - tf_mng_lookup_all/2 - ]). - -% define some settings -:- setting(use_logger, boolean, true, - 'Toggle whether TF messages are logged into the mongo DB.'). - -%% -:- mng_db_name(DB), - ( setting(tf:use_logger,false) - -> true - ; tf_logger_set_db_name(DB) - ). - -%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%% TF REPUBLISHER -%%%%%%%%%%%%%%%%%%%%%%% - -%% tf_republish_set_goal(+TimeStart, +TimeEnd) is det. -% -% Set a new goal for the TF republisher. -% The republisher then iterates over database records -% within the time interval provided. -% -tf_republish_set_goal(Time_min, Time_max) :- - tf_mongo:tf_db(DBName, CollectionName), - ( number(Time_min) - -> Min is Time_min - ; atom_number(Time_min,Min) - ), - ( number(Time_max) - -> Max is Time_max - ; atom_number(Time_max,Max) - ), - % initialize poses to last pose before Min. - % TODO: might be faster to do this in C++ code of republisher. - tf_republish_load_transforms(Min), - % start republishing range [Min,Max] - tf_republish_set_goal(DBName, CollectionName, Min, Max). - -%% -tf_republish_load_transforms(Time) :- - tf_mng_lookup_all(Transforms, Time), - forall( - ( member([Ref,Frame,Pos,Rot],Transforms), - Ref \= Frame, - \+ atom_concat('/',Ref,Frame), - \+ atom_concat('/',Frame,Ref) - ), - tf_republish_set_pose(Frame,[Ref,Pos,Rot]) - ). - -%% tf_republish_set_progress(+Progress) is det. -% -% Advance republisher to some point in time. -% Progress is a number between zero and one used -% as interpolation factor between start and end time. -% - -%% tf_republish_clear is det. -% -% Reset the TF republisher. -% - -%% tf_republish_set_loop(+Loop) is det. -% -% Toggle looping from end time to start time. -% - -%% tf_republish_set_time(+Time) is det. -% -% Set the current time of the TF republisher. -% - -%% tf_republish_set_pose(+ObjFrame, +PoseData) is det. -% -% Update the transform of a frame in the TF republisher. -% This is useful to initialize transforms from data not within -% the range of the republisher. -% - -%% tf_republish_set_realtime_factor(+Factor) is det. -% -% Change the realtime factor. -% Default is 1.0, i.e. realtime republishing. -% - -%% tf_logger_enable is det. -% -% Activate the TF logger. -% That is start listening to TF messages and storing -% them in a mongo database. -% - -%% tf_logger_disable is det. -% -% Deactivate the TF logger. -% - -%% tf_mem_clear is det. -% -% Reset the TF memory. -% That is remove every transform stored in the local memory. -% - -%% tf_mem_set_pose(+ObjFrame,+PoseData,+Since) is det. -% -% Update the transform of a frame in local memory. -% - -%% tf_mem_get_pose(+ObjFrame,?PoseData,?Since) is det. -% -% Read the transform of a frame from local memory. -% - -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % is_at - -% add is_at/2 as computable predicate -:- add_computable_predicate(is_at/2, tf:tf_get_pose). - -%% tf_set_pose(+Obj, +Data, +Scope) is det. -% -% Update the position of an object. -% The position is updated in local memory, but also -% written into mongo database. -% -tf_set_pose(Obj,PoseData,FS) :- - rdf_split_url(_,ObjFrame,Obj), - time_scope_data(FS,[Since,_Until]), - tf_mem_set_pose(ObjFrame,PoseData,Since), - tf_mng_store(ObjFrame,PoseData,Since). - -%% -tf_get_pose(Obj,[RefFrame,Pos,Rot]) :- - current_scope(QS), - tf_get_pose(Obj,[RefFrame,Pos,Rot],QS,_FS). - -%% tf_get_pose(+Obj, ?PoseQuery, +QueryScope, +FactScope) is semidet. -% -% Retrieve the pose of an object. -% The pose can be requested in a specific reference frame. -% -tf_get_pose(Obj,PoseQuery,QS,FS) :- - rdf_split_url(_,ObjFrame,Obj), - % lookup direct position data without doing transformations - is_at_direct(ObjFrame,PoseData,QS,FS0), - % then try to unify the query - ( PoseQuery=PoseData - -> FS=FS0 - % else try to compute the position in the requested frame - ; is_at_indirect(ObjFrame,PoseQuery,PoseData,QS,FS1) - -> scope_intersect(FS0,FS1,FS) - ; fail - ). - -%% -is_at_direct(ObjFrame,PoseData,QS,FS) :- - % get local pose data and scope - tf_mem_get_pose(ObjFrame,PoseData,Since), - get_time(Now), - time_scope(=(Since),=(Now),FS), - % make sure there is an overlap with the query scope - time_scope_data(QS,[QSince,QUntil]), - mng_strip_operator(QSince, _, QSince0), - mng_strip_operator(QUntil, _, QUntil0), - time_scope(QSince0,=(QUntil0),=(QS0)), - scope_intersect(FS,QS0,_), - % skip other results in case the fact scope is a superscope of the query scope - ( subscope_of(QS,FS) - -> ! - ; true - ). - -is_at_direct(ObjFrame,PoseData,QS,FS) :- - time_scope_data(QS,[QSince,QUntil]), - mng_strip_operator(QSince, _, QSince0), - mng_strip_operator(QUntil, _, QUntil0), - % FIXME: this is pretty slow for "deep" trees as for each level one call of is_at_direct is made. - % problem is that it is difficult to find all parents of a frame in mongo aggregation. - % - $graphLookup would need to be used, but with additional filter to only follow the one path (even possible?) - % but if possible it would safe a lot IO here. - % - or change datamodel to include this info in each document - % - tf_mng_lookup(ObjFrame,QSince0,QUntil0,PoseData,FSince,FUntil), - time_scope(=(FSince),=(FUntil),FS). - -%% -is_at_indirect(ObjFrame,PoseQuery,DirectPose,QS,FS) :- - % get object pose in world frame - world_pose1(ObjFrame,_,DirectPose,WorldPose,QS,FS0), - is_at_indirect1(ObjFrame,PoseQuery,WorldPose,FS0,QS,FS). - -is_at_indirect1(_ObjFrame, - [WorldFrame,T_query,Q_query], - [WorldFrame,T0,Q0], - FS, _QS, FS) :- - !, - % pose was requested in world frame - T_query = T0, - Q_query = Q0. - -is_at_indirect1(ObjFrame, - [RefFrame_query,T_query,Q_query], - [WorldFrame,T0,Q0], - FS0, QS, FS) :- - % get requested frame in world frame - world_pose(RefFrame_query,WorldFrame, - [WorldFrame,T1,Q1], - QS, FS1), - % - scope_intersect(FS0,FS1,FS), - % compute transform from requested frame - % to object frame - transform_between( - [WorldFrame, ObjFrame, T0,Q0], - [WorldFrame, RefFrame_query, T1,Q1], - [RefFrame_query, ObjFrame, T_query,Q_query] - ). - -%% -world_pose(ObjFrame,WorldFrame,MapPose,QS,FS) :- - is_at_direct(ObjFrame,ObjPose,QS,FS0), - world_pose1(ObjFrame,WorldFrame,ObjPose,MapPose,QS,FS1), - scope_intersect(FS0,FS1,FS). - -world_pose1(ObjFrame, WorldFrame, - [ParentFrame,T0,Q0], - [WorldFrame,T,Q], - QS, FS) :- - world_pose(ParentFrame, WorldFrame, [WorldFrame,T1,Q1], QS, FS), - !, - transform_multiply( - [WorldFrame,ParentFrame,T1,Q1], - [ParentFrame,ObjFrame,T0,Q0], - [WorldFrame,ObjFrame,T,Q] - ). - -world_pose1(_, WorldFrame, - [WorldFrame|Rest], - [WorldFrame|Rest], _, _). - diff --git a/src/tf/tf.plt b/src/tf/tf.plt deleted file mode 100755 index 267198a..0000000 --- a/src/tf/tf.plt +++ /dev/null @@ -1,131 +0,0 @@ -:- use_module(library('rostest')). -:- use_module(library('lang/query')). -:- use_module(library('scope')). -:- use_module(library('mongolog/client')). -:- use_module(library('semweb/rdf_db')). - -:- use_module('tf'). -:- use_module('tf_mongo'). - -:- begin_rdf_tests( - 'tf', - 'package://knowrob/owl/test/swrl.owl', - [ setup(tf_setup), cleanup(tf_cleanup) ]). - -:- sw_register_prefix(test, 'http://knowrob.org/kb/swrl_test#', [force(true)]). - -:- rdf_meta(test_set_pose(r,+,+)). -:- rdf_meta(test_get_pose(r,+,+)). -:- rdf_meta(test_trajectory(r,+,+,+)). -:- rdf_meta(test_transform_pose(r,+,+)). -:- rdf_meta(test_is_unlocalized(r,+)). - -tf_setup :- - tf_mng_drop, - tf_logger_enable. - -tf_cleanup :- - tf_logger_disable, - tf_mng_drop. - -% some tests facts -test_pose_fred0([world,[1.0,0.4,2.32],[0.0,0.0,0.0,1.0]], 1593178679.123). -test_pose_fred1([world,[2.0,0.4,2.32],[0.0,0.0,0.0,1.0]], 1593178680.123). -test_pose_fred2([world,[3.0,0.4,2.32],[0.0,0.0,0.0,1.0]], 1593178681.123). -test_pose_alex1(['Fred',[0.0,1.0,0.0],[0.0,0.0,0.0,1.0]], 1593178680.123). -test_pose_alex2([world,[0.0,0.0,1.0],[0.0,0.0,0.0,1.0]], 1593178681.123). -test_pose_alex3(['Fred',[0.0,1.0,0.0],[0.0,0.0,0.0,1.0]], 1593178682.123). - -test_set_pose(Object,Pose,Stamp) :- - time_scope(=(Stamp), =<('Infinity'), FScope), - assert_true(tf:tf_set_pose(Object,Pose,FScope)). - -test_get_pose(Object,Stamp,Expected) :- - time_scope(=<(Stamp), >=(Stamp), QScope), - assert_true(tf:tf_get_pose(Object,_,QScope,_)), - ( tf:tf_get_pose(Object,Actual,QScope,_) - -> assert_unifies(Actual,Expected) - ; true - ). - -test_trajectory(Obj,Begin,End,Expected) :- - assert_true(tf_mongo:tf_mng_trajectory(Obj,Begin,End,_)), - ( tf_mongo:tf_mng_trajectory(Obj,Begin,End,Actual) - -> assert_unifies(Actual,Expected) - ; true - ). - -test_transform_pose(Object,Stamp,Query) :- - time_scope(=<(Stamp), >=(Stamp), QScope), - assert_true(tf:tf_get_pose(Object,Query,QScope,_)). - -test_lookup(Frame,Stamp,Expected) :- - assert_true(tf:tf_mng_lookup(Frame,Stamp,Stamp,_,_,_)), - ( tf:tf_mng_lookup(Frame,Stamp,Stamp,Actual,_,_) - -> assert_unifies(Actual,Expected) - ; true - ). - -test_lookup_fails(Frame,Stamp) :- - assert_false(tf:tf_mng_lookup(Frame,Stamp,Stamp,_,_,_)). - -test_is_unlocalized(Object,Stamp) :- - time_scope(=<(Stamp), >=(Stamp), QScope), - assert_false(tf:tf_get_pose(Object,_,QScope,_)). - -test('tf_pose') :- - test_pose_fred0(Pose0,Stamp0), - Past is Stamp0 - 1.0, - Future is Stamp0 + 1.0, - %% - test_is_unlocalized(test:'Fred',Stamp0), - test_set_pose(test:'Fred',Pose0,Stamp0), - test_get_pose(test:'Fred',Stamp0,Pose0), - test_get_pose(test:'Fred',Future,Pose0), - test_is_unlocalized(test:'Fred',Past). - -test('tf_mongo_lookup') :- - test_pose_fred0(Pose0,Stamp0), - Past is Stamp0 - 1.0, - test_lookup('Fred',Stamp0,Pose0), - test_lookup_fails('Fred',Past). - -test('tf_pose_memory') :- - test_pose_fred0(Pose0,Stamp0), - test_pose_fred1(Pose1,Stamp1), - test_pose_fred2(Pose2,Stamp2), - Stamp01 is 0.5*(Stamp0 + Stamp1), - Future is Stamp2 + 1.0, - %% - test_set_pose(test:'Fred',Pose1,Stamp1), - test_set_pose(test:'Fred',Pose2,Stamp2), - %% - test_get_pose(test:'Fred',Stamp0,Pose0), - test_get_pose(test:'Fred',Stamp01,Pose0), - test_get_pose(test:'Fred',Stamp1,Pose1), - test_get_pose(test:'Fred',Stamp2,Pose2), - test_get_pose(test:'Fred',Future,Pose2). - -test('tf_trajectory') :- - test_pose_fred0(Pose0,Stamp0), - test_pose_fred1(Pose1,Stamp1), - test_pose_fred2(Pose2,Stamp2), - test_trajectory(test:'Fred',Stamp0,Stamp2, - [Stamp0-Pose0, Stamp1-Pose1, Stamp2-Pose2]), - test_trajectory(test:'Fred',Stamp1,Stamp2, - [Stamp1-Pose1, Stamp2-Pose2]). - -test('tf_transform_pose') :- - test_pose_alex1(Pose1,Stamp1), - test_set_pose(test:'Alex',Pose1,Stamp1), - test_get_pose(test:'Alex',Stamp1,Pose1), - test_transform_pose(test:'Alex',Stamp1,[world,[2.0,1.4,2.32],_]), - test_transform_pose(test:'Fred',Stamp1,['Alex',_,_]). - -%tests('tf_is_at') :- -% test_pose_fred0(Pose0,Stamp0), -% assert_true(kb_call( -% during(is_at(tests:'Fred',Pose0), [Stamp0,Stamp0]) -% )). - -:- end_tests('tf'). diff --git a/src/tf/tf_mongo.pl b/src/tf/tf_mongo.pl deleted file mode 100644 index b488a33..0000000 --- a/src/tf/tf_mongo.pl +++ /dev/null @@ -1,319 +0,0 @@ -:- module(tf_mongo, - [ tf_mng_store/3, - tf_mng_lookup/6, - tf_mng_lookup_all/1, - tf_mng_lookup_all/2, - tf_mng_trajectory/4, - tf_mng_drop/0, - tf_mng_tree/2, - tf_mng_tree_lookup/3 - ]). - -:- use_foreign_library('libtf_knowrob.so'). - -:- use_module(library('semweb/rdf_db'), - [ rdf_split_url/3 ]). -:- use_module(library('utility/algebra'), - [ transform_between/3, - transform_multiply/3 - ]). -:- use_module(library('mongolog/client')). - -% stores the last TF tree constructed from mongo -:- dynamic tree_cache/2. - -% register tf_raw/9 as a fluent predicate -% FIXME: need to update collection name if collection prefix is changed -:- mng_get_db(_DB, CollectionName, 'tf'), - mongolog_add_fluent(tf_raw, - % predicate arguments - [ +'child_frame_id' % +ChildFrame - , -'header.frame_id' % -ParentFrame - , -'transform.translation.x' % -X - , -'transform.translation.y' % -Y - , -'transform.translation.z' % -Z - , -'transform.rotation.x' % -QX - , -'transform.rotation.y' % -QY - , -'transform.rotation.z' % -QZ - , -'transform.rotation.w' % -QW - ], - % time field - 'header.stamp', - % options - [ collection(CollectionName) - ]). - -% add *tf* as a mongolog command such that it can be -% used in mongolog queries. -:- mongolog:add_command(tf,4). - -% tf/4 -lang_query:step_expand( - tf(ChildFrame, ParentFrame, [X,Y,Z], [QX,QY,QZ,QW]), - tf_raw(ChildFrame, ParentFrame, X, Y, Z, QX, QY, QZ, QW)). - -lang_query:step_expand( - assert(tf(ChildFrame, ParentFrame, [X,Y,Z], [QX,QY,QZ,QW])), - assert(tf_raw(ChildFrame, ParentFrame, X, Y, Z, QX, QY, QZ, QW))). - -% tf/2 -lang_query:step_expand( - tf(ChildFrame, [ParentFrame, [X,Y,Z], [QX,QY,QZ,QW]]), - tf_raw(ChildFrame, ParentFrame, X, Y, Z, QX, QY, QZ, QW)). - -lang_query:step_expand( - assert(tf(ChildFrame, [ParentFrame, [X,Y,Z], [QX,QY,QZ,QW]])), - assert(tf_raw(ChildFrame, ParentFrame, X, Y, Z, QX, QY, QZ, QW))). - -%% -tf_db(DB, Name) :- - mng_get_db(DB, Name, 'tf'). - -%% tf_mng_drop is det. -% -% Drops all documents in the TF database collection. -% -tf_mng_drop :- - tf_db(DB, Name), - mng_drop(DB,Name). - -%% tf_mng_lookup(+ObjFrame, +QuerySince, +QueryUntil, -PoseData, -FactSince, -FactUntil) is nondet. -% -% Retrieve all transforms of frame within some time interval. -% -tf_mng_lookup( - ObjFrame, Query_Since, Query_Until, - PoseData, Fact_Since, Fact_Until) :- - % create a query scope - time_scope(=<(Query_Since), >=(Query_Until), QueryScope), - mongolog_call( - tf(ObjFrame, PoseData), - [ scope(QueryScope), - user_vars([['v_scope',FactScope]]) - ]), - % read values from FactScope - time_scope(double(Fact_Since), double(Fact_Until), FactScope). - -%% tf_mng_lookup(+ObjFrame, -PoseData) is nondet. -% -% Retrieve the latest transform of a frame. -% -tf_mng_lookup(ObjFrame, PoseData) :- - get_time(Now), - tf_mng_lookup(ObjFrame, Now, Now, PoseData, _, _). - -%% tf_mng_store(+ObjFrame, +PoseData, +Stamp) is det. -% -% Store a transform in mongo DB. -% - - -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % trajectories - -%% tf_mng_trajectory(+Obj, +Begin, +End, -Trajectory) is det. -% -% Read all transforms associated to a frame in given time interval. -% -tf_mng_trajectory(Obj,Stamp0,Stamp1,Trajectory) :- - rdf_split_url(_,ObjFrame,Obj), - findall(Stamp-Data, - ( tf_mng_lookup(ObjFrame,Stamp0,Stamp1,Data,Stamp,_), - Stamp >= Stamp0, - Stamp =< Stamp1 - ), - Trajectory0 - ), - reverse(Trajectory0,Trajectory). - - -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % bulk lookup - - -%% tf_mng_lookup_all(-Transforms) is det. -% -% Retrieve latest transform of each frame. -% -tf_mng_lookup_all(Transforms) :- - tf_mng_lookup_all(Transforms, _). - - -%% tf_mng_lookup_all(-Transforms, +Stamp) is det. -% -% Retrieve latest transform of each frame. -% Documents later than Stamp are ignored. -% -tf_mng_lookup_all(Transforms, Stamp) :- - tf_db(DB,Coll), - % first create lookup $match steps - findall(MatchStep, - ( MatchStep=['$eq',array([string('$child_frame_id'),string('$$frame')])] - ; ( ground(Stamp), - MatchStep=['$lt',array([string('$header.stamp'),time(Stamp)])] - ) - ), - MatchSteps), - % create aggregate pipeline - % TODO: $facet may can be used to avoid $lookup here - % TODO: integrate this with mongolog fluents? - % i.e. when key fields are queried, then $group them - findall(Step, - % get distinct frames - ( Step=['$group',['_id',['child_frame_id',string('$child_frame_id')]]] - ; Step=['$lookup',[ - ['from',string(Coll)], - ['as',string('tf')], - ['let',['frame',string('$_id.child_frame_id')]], - ['pipeline',array([ - ['$match',['$expr',['$and',array(MatchSteps)]]], - ['$sort',[ - ['child_frame_id',int(1)], - ['header.stamp',int(-1)] - ]], - ['$limit',int(1)] - ])] - ]] - ; Step=['$unwind',string('$tf')] - ; Step=['$replaceRoot',['newRoot',string('$tf')]] - ), - Pipeline - ), - %% - setup_call_cleanup( - mng_cursor_create(DB,Coll,Cursor), - ( mng_cursor_aggregate(Cursor,['pipeline',array(Pipeline)]), - findall([Ref,Frame,Pos,Rot], - ( mng_cursor_materialize(Cursor,Doc), - tf_mng_doc_pose(Doc,Frame,_,[Ref,Pos,Rot]) - ), - Transforms - ) - ), - mng_cursor_destroy(Cursor) - ). - - -%% -% Convert mongo document to pose term. -% -tf_mng_doc_pose(Doc,ObjFrame,Time,[ParentFrame,[TX,TY,TZ],[QX,QY,QZ,QW]]) :- - get_dict(child_frame_id, Doc, string(ObjFrame)), - get_dict(header,Doc, - [ _, stamp-time(Time), frame_id-string(ParentFrame) ] - ), - get_dict(transform,Doc,[ - translation-[ x-double(TX), y-double(TY), z-double(TZ) ], - rotation-[ x-double(QX), y-double(QY), z-double(QZ), w-double(QW) ] - ]). - - -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -% % % % % TF tree - - -%% tf_mng_tree(+Stamp, -Tree) is semidet. -% -% Loads full TF tree for given timestamp. -% All poses in the tree are transformed into world coordinates. -% -tf_mng_tree(Stamp, Tree) :- - tree_cache(Stamp, Tree), - !. - -tf_mng_tree(Stamp, Tree) :- - % read all required transforms - tf_mng_lookup_all(Transforms, Stamp), - % create a tree structure for traversal operation - tree_create(Transforms, Tree0), - % compute world poses - tree_traverse(Tree0, Tree1), - % create dictionary for faster lookup - tree_dict(Tree1, Tree), - % cache tree - retractall(tree_cache(_,_)), - assertz(tree_cache(Stamp, Tree)). - -%% tf_mng_tree_lookup(+Tree, +ChildFrame, -PoseData) is semidet. -% -% Retrieve pose of named frame from a previously created TF tree. -% PoseData is a list `[RefFrame,Position,Rotation]`. -% RefFrame maybe grounded in which case the requested relative -% transform is computed if needed and possible. -% -tf_mng_tree_lookup(Tree, ChildFrame, [RefFrame,Pos,Rot]) :- - get_dict(ChildFrame, Tree, [RefFrame0,Pos0,Rot0]), - % return identity of transform if possible - ( RefFrame=RefFrame0 -> ( Pos=Pos0, Rot=Rot0 ) - % else compute transform wrt. requested reference frame - ; ( get_dict(RefFrame, Tree, [RefFrame0,Pos1,Rot1]), - transform_between( - [RefFrame0,ChildFrame,Pos0,Rot0], - [RefFrame0,RefFrame,Pos1,Rot1], - [RefFrame,ChildFrame,Pos,Rot]) - ) - ), - !. - -%% -tree_create(Transforms,Tree) :- - % gather all childrens - findall(Frame-[Parent,Pos,Rot,Children], - ( member([Parent,Frame,Pos,Rot],Transforms), - findall(X, member([Frame,X,_,_],Transforms), Children) - ), - Frames - ), - dict_pairs(Frames_dict,_,Frames), - % create the tree - findall(Branch, - ( member([Root,Frame,_,_],Transforms), - \+ get_dict(Root,Frames_dict,_), - tree_create_branch(Frame,Frames_dict,Branch) - ), - Tree - ). - -%% -tree_create_branch(Frame,Dict,(Pose,Children)) :- - get_dict(Frame,Dict,[Parent,Pos,Rot,Children0]), - Pose=[Parent,Frame,Pos,Rot], - findall(X, - ( member(ChildFrame,Children0), - tree_create_branch(ChildFrame,Dict,X) - ), - Children). - -%% -tree_traverse([], []) :- !. -tree_traverse([X|Xs], [Y|Ys]) :- - tree_traverse_0(X,Y), - tree_traverse(Xs,Ys). - -%% -tree_traverse_0((Pose,[]), (Pose,[])):- !. -tree_traverse_0((Pose,[X|Xs]), (Pose,[Y|Ys])):- - tree_traverse_1(Pose,X,Y), - tree_traverse_0((Pose,Xs), (Pose,Ys)). - -%% -tree_traverse_1(Pose_parent, (Pose_child,Xs), (Pose,Ys)) :- - % get child pose in world frame - transform_multiply(Pose_parent,Pose_child,Pose), - % continue for children - tree_traverse_0((Pose,Xs),(Pose,Ys)). - -%% -tree_dict(Tree,Dict) :- - findall(Pair, tree_dict_1(Tree,Pair), Pairs), - dict_pairs(Dict,_,Pairs). - -%% -tree_dict_1( - [([RefFrame,ChildFrame,Pos,Rot],_)|_], - ChildFrame-[RefFrame,Pos,Rot]). -tree_dict_1([(_,Xs)|_],Pair) :- tree_dict_1(Xs,Pair). -tree_dict_1([_|Xs],Pair) :- tree_dict_1(Xs,Pair). diff --git a/src/urdf/README.md b/src/urdf/README.md deleted file mode 100644 index c22ba78..0000000 --- a/src/urdf/README.md +++ /dev/null @@ -1,18 +0,0 @@ -\page ros_urdf URDF Integration - -A Prolog-based interface to the Unified Robot Description Format (URDF). -The package first implements a wrapper around the C++ URDF parser library, -and second a mapping into a OWL model of URDF. -The OWL model is defined in an ontology that comes with this package. - -Here is example Prolog code where urdfprolog generates RDF descriptions -of a PR2 robot: - - ros_package_path('urdfprolog', X), - atom_concat(X, '/urdf/pr2_for_unit_tests.urdf', FileURL), - kb_create(urdf:'Robot', Robot), - rdf_urdf_load(Robot, FileURL). - - The result is a RDF description of the robot's links and joints. - However, components are not considered in URDF. - For component descriptions, please have a look at the SRDL package. diff --git a/src/urdf/URDF.pl b/src/urdf/URDF.pl deleted file mode 100755 index 6dd3b78..0000000 --- a/src/urdf/URDF.pl +++ /dev/null @@ -1,520 +0,0 @@ -:- module(ros_urdf, - [ urdf_load(r,+), - urdf_load(r,+,+), - has_urdf(r,r), - has_base_link_name(r,?), - has_end_link_name(r,?), - has_base_link(r,r), - has_end_link(r,r), - has_child_link(r,r), - has_parent_link(r,r), - urdf_set_pose(r,+), - urdf_set_pose_to_origin(r,+), - urdf_robot_name/2, - urdf_link_names/2, - urdf_joint_names/2, - urdf_root_link/2, - urdf_link_parent_joint/3, - urdf_link_child_joints/3, - urdf_link_inertial/5, - urdf_link_visual_shape/6, - urdf_link_collision_shape/4, - urdf_joint_type/3, - urdf_joint_child_link/3, - urdf_joint_parent_link/3, - urdf_joint_axis/3, - urdf_joint_origin/3, - urdf_joint_calibration_falling/3, - urdf_joint_calibration_rising/3, - urdf_joint_hard_limits/5, - urdf_joint_soft_limits/5, - urdf_joint_damping/3, - urdf_joint_friction/3, - is_urdf_link(r), - is_urdf_joint(r), - urdf_init/0 - ]). - -:- use_module(library('semweb/rdf_db'), - [ rdf_split_url/3, rdf_meta/1 ]). -:- use_module(library('lang/db'), - [ load_owl/2 ]). -:- use_module(library('lang/query')). -:- use_module(library('utility/url'), [ url_resolve/2 ]). -:- use_module(library('utility/filesystem'), [ path_concat/3 ]). -:- use_module(library(http/http_client)). -:- use_module(library('ros/tf/tf'), [ tf_mem_set_pose/3 ]). - -:- use_foreign_library('liburdf_parser.so'). - -:- load_owl('http://knowrob.org/kb/URDF.owl', - [ namespace(urdf,'http://knowrob.org/kb/urdf.owl#') - ]). - -%% has_urdf(+Object,+RootObject) is semidet. -% -% True if Object is a part of RootObject, and RootObject is assigned -% to some URDF description. -% -% @param Part IRI atom -% @param Root IRI atom -% -:- dynamic has_urdf/2. -:- dynamic urdf_server/1. -:- dynamic urdf_prefix/2. - -%% -% Initialize prefix for downloading URDF via HTTP -%% -urdf_server_init :- - once(( - getenv('KNOWROB_URDF_SERVER', URL) - ; URL='http://neem-data.informatik.uni-bremen.de/data/kinematics/' - )), - retractall(urdf_server(_)), - assertz(urdf_server(URL)). -:- urdf_server_init. - -%% -% -% -urdf_init :- - retractall(has_urdf(_,_)), - retractall(urdf_prefix(_,_)), - forall( - has_kinematics_file(Object,Identifier,'URDF'), - urdf_init(Object,Identifier) - ). - -urdf_init(Object,_) :- - has_urdf(Object,_), - !. - -urdf_init(Object,Identifier) :- - urdf_server(DATA_URL), - atomic_list_concat([Identifier,urdf],'.',Filename), - path_concat(DATA_URL,Filename,URL), - % get XML data - ( http_get(URL,XML_data,[]) -> true - ; ( log_warn(urdf(download_failed(Object,URL))), - fail - ) - ), - % parse data - ( urdf_load_xml(Object,XML_data) -> true - ; ( log_warn(urdf(parsing_failed(Object,URL))), - fail - ) - ), - % create has_urdf facts - forall( - ( Y=Object - ; kb_call(triple(Object,transitive(dul:hasComponent),Y)) - ), - ( has_urdf(Y,Object) -> true - ; assertz(has_urdf(Y,Object)) - ) - ), - log_info(urdf(initialized(Object,Identifier))), - !. - -%% urdf_load(+Object,+File) is semidet. -% -% Same as urdf_load/3 with empty options list. -% -% @param Object IRI atom -% @param File Path to URDF file -% -urdf_load(Object,File) :- - urdf_load(Object,File,[]). - -%% urdf_load(+Object,+File,+Options) is semidet. -% -% Assert a URDF file as description of an object. -% This is primarly done to associate parts of the object -% to links described in the URDF file. -% Links and joints in the URDF file may further be mapped -% to a RDF model and asserted to the triple store. -% -% @param Object IRI atom -% @param File Path to URDF file -% @param Options List of options -% -urdf_load(Object,URL,Options) :- - ( url_resolve(URL,Resolved) - -> true - ; Resolved=URL - ), - urdf_load_file(Object,Resolved), - % create IO and IR objects in triple store - file_base_name(Resolved,FileName), - file_name_extension(Identifier,_,FileName), - kb_project(has_kinematics_file(Object,Identifier,'URDF')), - % assign urdf name to object - urdf_root_link(Object,RootLinkName), - kb_project(has_base_link_name(Object,RootLinkName)), - % assign prefix to object - option(prefix(OptPrefix),Options,''), - ( OptPrefix='' - -> true - ; kb_project(has_urdf_prefix(Object,OptPrefix)) - ), - % get all the object parts - findall(X, kb_call(triple(Object,transitive(dul:hasComponent),X)), Parts), - % set component poses relative to links - forall( - ( member(Y,[Object|Parts]), - has_base_link_name(Y,YName) - ), - ( atom_concat(OptPrefix,YName,YFrame), - % update tf memory - rdf_split_url(_,OName,Y), - % do not create a frame for the entity if its name is equal to frame name. - (OName == YFrame -> true ; - tf_mem_set_pose(OName, [YFrame,[0,0,0],[0,0,0,1]], 0)), - % - assertz(has_urdf(Y,Object)) - ) - ), - % optional: load links and joints as rdf objects - ( option(load_rdf,Options) - -> load_rdf_(Object,Parts,OptPrefix) - ; true - ). - -%% urdf_set_pose_to_origin(+Object,+Frame) is semidet. -% -% Same as urdf_set_pose/2 but assigns the base of the -% URDF to be located exactly at the frame given in the second argument. -% -% @param Object IRI atom -% @param Frame URDF frame name atom -% -urdf_set_pose_to_origin(Object,Frame) :- - urdf_set_pose(Object,[Frame,[0,0,0],[0,0,0,1]]). - -%% urdf_set_pose(+Object,+Pose) is semidet. -% -% Assign the initial pose of links described -% in URDF as the current pose of link entities. -% This requires that the *load_rdf* option was used in urdf_load/3. -% The base link of the URDF is assigned to the transform given -% in the second argument. -% -% @param Object IRI atom -% @param Frame A pose list of frame-position-quaternion -% -urdf_set_pose(Object,Pose) :- - ( has_urdf_prefix(Object,Prefix) - ; Prefix='' - ),!, - % set root link pose - urdf_root_link(Object,RootLinkName), - urdf_iri(Object,Prefix,RootLinkName,RootLink), - % update tf memory - rdf_split_url(_,RootFrame,RootLink), - tf_mem_set_pose(RootFrame,Pose,0), - % set pose of other links - urdf_link_names(Object,Links), - forall( - member(LinkName,Links), - ( LinkName=RootLinkName - ; set_link_pose_(Object,Prefix,LinkName) - ) - ). - -set_link_pose_(Object,Prefix,LinkName) :- - urdf_link_parent_joint(Object,LinkName,JointName), - urdf_joint_origin(Object,JointName,[_,Pos,Rot]), - urdf_joint_parent_link(Object,JointName,ParentName), - urdf_iri(Object,Prefix,LinkName,Link), - atom_concat(Prefix,ParentName,ParentFrame), - % update tf memory - rdf_split_url(_,LinkFrame,Link), - tf_mem_set_pose(LinkFrame, [ParentFrame,Pos,Rot], 0). - -%% -% -urdf_link_visual_shape(Object,Link,ShapeTerm,Origin,MaterialTerm,ShapeID) :- - urdf_link_num_visuals(Object,Link,Count), - N is Count - 1, - between(0,N,Index), - atom_concat(Link,Index,ShapeID), - urdf_link_nth_visual_shape(Object,Link,Index,ShapeTerm,Origin,MaterialTerm). - -%% -% -urdf_link_collision_shape(Object,Link,ShapeTerm,Origin) :- - urdf_link_num_collisions(Object,Link,Count), - N is Count - 1, - between(0,N,Index), - urdf_link_nth_collision_shape(Object,Link,Index,ShapeTerm,Origin). - -%% -urdf_chain(_,X,X,X) :- !. -urdf_chain(_,_,X,X). -urdf_chain(Object,FromLink,ToLink,Node) :- - urdf_link_parent_joint(Object,ToLink,Joint), - urdf_joint_parent_link(Object,Joint,ParentLink), - urdf_chain(Object,FromLink,ParentLink,Node). - -/**************************************/ -/********* RDF REPRESENTATION *********/ -/**************************************/ - -%% -urdf_iri(Object,URDFPrefix,Name,IRI) :- - %% - % FIXME: BUG: providing a prefix with '/' breaks rdf_split_url - % for link individuals!! - %% - rdf_split_url(IRIPrefix,_,Object), - %atomic_list_concat([IRIPrefix,URDFPrefix,Name],'',IRI), - atomic_list_concat([IRIPrefix,Name],'',IRI). - -%% -:- rdf_meta(joint_type_iri(?,r)). -joint_type_iri(revolute, urdf:'RevoluteJoint'). -joint_type_iri(continuous, urdf:'ContinuousJoint'). -joint_type_iri(prismatic, urdf:'PrismaticJoint'). -joint_type_iri(fixed, urdf:'FixedJoint'). -joint_type_iri(floating, urdf:'FloatingJoint'). -joint_type_iri(planar, urdf:'PlanarJoint'). - -%% -load_rdf_(Object,Parts,Prefix) :- - % create link entities - urdf_link_names(Object,Links), - forall(member(LinkName,Links), create_link_(Object,Prefix,LinkName,_)), - % create joint entities - urdf_joint_names(Object,Joints), - forall(member(JointName,Joints), create_joint_(Object,Prefix,JointName,_)), - % associate links to components - forall(member(Part,Parts), set_links_(Part,Prefix)). - -%% -create_link_(Object,Prefix,Name,Link) :- - urdf_iri(Object,Prefix,Name,Link), - kb_project(has_type(Link,urdf:'Link')). - -%% -create_joint_(Object,Prefix,Name,Joint) :- - urdf_iri(Object,Prefix,Name,Joint), - %% - urdf_joint_type(Object,Name,Type), - joint_type_iri(Type,JointType), - %% - urdf_joint_child_link(Object,Name,ChildName), - urdf_joint_parent_link(Object,Name,ParentName), - urdf_iri(Object,Prefix,ChildName,Child), - urdf_iri(Object,Prefix,ParentName,Parent), - %% - kb_project([ - has_type(Joint,JointType), - has_child_link(Joint,Child), - has_parent_link(Joint,Parent) - ]). - -%% -set_links_(Part,Prefix) :- - ( has_base_link_name(Part,BaseLinkName) - -> ( urdf_iri(Part,Prefix,BaseLinkName,BaseLink), - (Part==BaseLink -> true ; kb_project(has_base_link(Part,BaseLink))) - ) - ; true - ),!, - forall( - has_end_link_name(Part,EndLinkName), - ( urdf_iri(Part,Prefix,EndLinkName,EndLink), - (Part==EndLink -> true ; kb_project(has_end_link(Part,EndLink))) - ) - ). - -/**************************************/ -/*********** LANG EXTENSIONS **********/ -/**************************************/ - -%% is_urdf_link(?Entity) is semidet. -% -is_urdf_link(Entity) ?+> - has_type(Entity, urdf:'Link'). - -%% is_urdf_joint(?Entity) is semidet. -% -is_urdf_joint(Entity) ?+> - has_type(Entity, urdf:'Joint'). - -%% has_urdf_prefix(?Obj,?Prefix) is semidet. -% -has_urdf_prefix(Obj,Prefix) ?+> - triple(Obj,urdf:hasNamePrefix,Prefix). - -%% has_urdf_name(?Obj,?Name) is semidet. -% -%has_urdf_name(Obj,Name) ?+> -% triple(Obj,urdf:hasURDFName,Name). - -%% has_base_link_name(?Obj,?Name) is semidet. -% -has_base_link_name(Obj,Name) ?+> - triple(Obj,urdf:hasBaseLinkName,Name). - -%% has_end_link_name(?Obj,?Name) is semidet. -% -has_end_link_name(Obj,Name) ?+> - triple(Obj,urdf:hasEndLinkName,Name). - -%% has_base_link(?Obj,?Link) is semidet. -% -has_base_link(Obj,Link) ?+> - triple(Obj,urdf:hasBaseLink,Link). - -%% has_end_link(?Obj,?Link) is semidet. -% -has_end_link(Obj,Link) ?+> - triple(Obj,urdf:hasEndLink,Link). - -%% has_child_link(?Joint,?Link) is semidet. -% -has_child_link(Joint,Link) ?+> - triple(Joint,urdf:hasChildLink,Link). - -%% has_parent_link(?Joint,?Link) is semidet. -% -has_parent_link(Joint,Link) ?+> - triple(Joint,urdf:hasParentLink,Link). - -%% -% TODO: reconsider this -% -model_SOMA:object_shape(Obj,ShapeID,ShapeTerm,Origin,MaterialTerm) :- - object_shape_urdf(Obj,ShapeID,ShapeTerm,Origin,MaterialTerm). - -%% -object_shape_urdf(Obj,ShapeID,ShapeTerm,Origin,MaterialTerm) :- - var(Obj),!, - kb_call(( - has_base_link_name(Obj,BaseName), - has_end_link_name(Obj,EndName) - )), - has_urdf(Obj,Root), - get_object_shape_( - Root, BaseName, EndName, - ShapeID, ShapeTerm, Origin, MaterialTerm). - -object_shape_urdf(Obj,ShapeID,ShapeTerm,Origin,MaterialTerm) :- - %nonvar(Obj), - has_urdf(Obj,Root), - kb_call(( - has_base_link_name(Obj,BaseName), - has_end_link_name(Obj,EndName) - )), - get_object_shape_( - Root, BaseName, EndName, - ShapeID, ShapeTerm, Origin, MaterialTerm). - -%% -get_object_shape_(Root,BaseName,EndName, - ShapeID,ShapeTerm,[Frame,Pos,Rot],MaterialTerm) :- - % read prefix from root entity of URDF - once((urdf_prefix(Root,Prefix);( - (has_urdf_prefix(Root,Prefix);Prefix=''), - assertz(urdf_prefix(Root,Prefix)) - ))), - urdf_catch(urdf_chain(Root,BaseName,EndName,LinkName)), - urdf_catch(urdf_link_visual_shape(Root,LinkName, - ShapeTerm,[Name,Pos,Rot],MaterialTerm,ShapeID)), - atom_concat(Prefix,Name,Frame). - -%% -urdf_catch(Goal) :- - catch( - call(Goal), - urdf_error(Err), - ( print_message(error,urdf_error(Err)), - fail - ) - ). - -/**************************************/ -/********** FOREIGN LIBRARY ***********/ -/**************************************/ - -%% urdf_load_file(+Object,+Filename) is semidet. -% -% Load URDF from disc using global filename. - -%% urdf_unload_file(+Object) is semidet. -% -% Unloads a previously loaded URDF. - -%% urdf_robot_name(+Object,-Name) is semidet. -% -% Get the name of the currently loaded robot. - -%% urdf_root_link(+Object,-Name) is semidet. -% -% Get the name of the root link of the robot. - -%% urdf_link_parent_joint(+Object,+LinkName, -JointName) is semidet. -% -% Get the name of the parent joint of a link. - -%% urdf_link_child_joints(+Object,+LinkName, -JointNames) is semidet. -% -% Get the list of joint names of all child joints of a link. - -%% urdf_link_inertial(+Object, +LinkName, -Inertia, -Origin, -Mass) is semidet. -% -% Get the inertial origin of a link as a pose w.r.t. -% the origin frame of a link. -% -% Inertia matrices are coded as a list: -% [XX, XY, XZ, YY, YZ, ZZ]. -% For an explanation, visit: http://wiki.ros.org/urdf/XML/link - -%% urdf_joint_type(+Object,+JointName, Type) is semidet. -% -% Get the type of a joint. -% Possible types: revolute, prismatic, continuous, fixed, -% floating, planar, and unknown. - -%% urdf_joint_child_link(+Object,+JointName, -LinkName) is semidet. -% -% Get the name of the link of a joint. - -%% urdf_joint_parent_link(+Object,+JointName, -LinkName) is semidet. -% -% Get the name the parent link of a joint. - -%% urdf_joint_axis(+Object,+JointName, -Axis) is semidet. -% -% Get the axis of a joint, expressed as a list [X, Y, Z]. - -%% urdf_joint_origin(+Object,+JointName, -Origin) is semidet. -% -% Get the origin of a joint, expressed as a pose -% w.r.t. the link frame of its parent link. -% -% Poses are coded as a compound term: pose([X,Y,Z],[QX,QY,QZ,QW]), -% with the orientation represented as Quaternion. - -%% urdf_joint_calibration(+Object, +JointName, -Falling, -Rising) is semidet. -% -% Read the falling and rising reference positions of a joint. - -%% urdf_joint_damping(+Object,+JointName, -Damping) is semidet. -% -% Read the damping value of a joint. - -%% urdf_joint_friction(+Object,+JointName, -Friction) is semidet. -% -% Get the static friction value of a joint. - -%% urdf_joint_hard_limits(+Object, +JointName, -PosLimits, -VelMax, -EffMax) is semidet. -% -% Read the hard limits of a joint. - -%% urdf_joint_soft_limits(+Object, +JointName, -PosLimits, -KP, -KV) is semidet. -% -% Read the soft limits of a joint. diff --git a/src/urdf/URDF.plt b/src/urdf/URDF.plt deleted file mode 100755 index 34a5b26..0000000 --- a/src/urdf/URDF.plt +++ /dev/null @@ -1,222 +0,0 @@ -:- begin_rdf_tests('ros_urdf', 'package://knowrob/owl/test/swrl.owl'). - -:- use_module(library('model/RDFS')). -:- use_module('URDF'). - -:- sw_register_prefix(test, 'http://knowrob.org/kb/swrl_test#', [force(true)]). - -test(load_urdf_file_pr2) :- - ros_package_path('knowrob', X), - atom_concat(X, '/urdf/pr2_for_unit_tests.urdf', Filename), - urdf_load_file(pr2,Filename). - -test(robot_name_pr2) :- - urdf_robot_name(pr2,pr2). - -test(root_link_name_pr2) :- - urdf_root_link(pr2,base_footprint). - -test(joint_child_link_pr2_torso_lift_joint) :- - urdf_joint_child_link(pr2,torso_lift_joint, torso_lift_link). - -test(joint_child_link_pr2_l_shoulder_pan_joint) :- - urdf_joint_child_link(pr2,l_shoulder_pan_joint, l_shoulder_pan_link). - -test(joint_child_link_pr2_nonexisting_joint) :- - catch(urdf_joint_child_link(pr2, foo, l_shoulder_pan_link), urdf_error(Msg), true), - ground(Msg). - -test(joint_parent_link_pr2_r_elbow_flex_joint) :- - urdf_joint_parent_link(pr2, r_elbow_flex_joint, r_upper_arm_roll_link). - -test(joint_parent_link_pr2_head_tilt_joint) :- - urdf_joint_parent_link(pr2, head_tilt_joint, head_pan_link). - -test(joint_parent_link_nonexisting_joint) :- - catch(urdf_joint_parent_link(pr2, bar, head_pan_link), urdf_error(Msg), true), - ground(Msg). - -test(joint_type_pr2_torso_lift_joint) :- - urdf_joint_type(pr2, torso_lift_joint, prismatic). - -test(joint_type_pr2_l_wrist_roll_joint) :- - urdf_joint_type(pr2, l_wrist_roll_joint, continuous). - -test(joint_type_pr2_r_shoulder_lift_joint) :- - urdf_joint_type(pr2, r_shoulder_lift_joint, revolute). - -test(joint_type_pr2_head_plate_frame_joint) :- - urdf_joint_type(pr2, head_plate_frame_joint, fixed). - -test(joint_child_link_and_link_parent_joint_pr2_left_arm, forall( - member(J,[l_shoulder_pan_joint, l_shoulder_lift_joint, l_upper_arm_roll_joint, - l_elbow_flex_joint, l_forearm_roll_joint, l_wrist_flex_joint, l_wrist_roll_joint]))) :- - urdf_joint_child_link(pr2,J,L), - urdf_link_parent_joint(pr2,L,J). - -% Observation: The root link of a robot never has a parent joint. -test(link_parent_joint_pr2_root_link, fail) :- - urdf_root_link(pr2,L), - urdf_link_parent_joint(pr2, L, _). - - -test(link_child_joints_pr2_torso_lift_link) :- - urdf_link_child_joints(pr2, torso_lift_link, Joints), - Joints == - [head_pan_joint, imu_joint, l_shoulder_pan_joint, l_torso_lift_side_plate_joint, - laser_tilt_mount_joint, r_shoulder_pan_joint, r_torso_lift_side_plate_joint]. - -test(joint_axis_pr2_l_shoulder_pan_joint) :- - urdf_joint_axis(pr2, l_shoulder_pan_joint, [0,0,1]). - -test(joint_axis_pr2_fixed_joint, fail) :- - urdf_joint_axis(pr2, head_plate_frame_joint, _). - -test(joint_origin_pr2_r_gripper_led_joint) :- - urdf_joint_origin(pr2, r_gripper_led_joint, pose([0.0513, 0.0, 0.0244], [0, 0, 0, 1])). - -test(joint_origin_pr2_l_foreaem_cam_optical_frame_joint) :- - urdf_joint_origin(pr2, l_forearm_cam_optical_frame_joint, - pose([0,0,0],[-0.5, 0.5, -0.5, 0.5])). - -test(joint_lower_limit_pr2_l_elbow_flex_joint) :- - urdf_joint_hard_limits(pr2, l_elbow_flex_joint, [-2.3213,_], _, _). - -test(joint_lower_limit_pr2_l_wrist_roll_joint, fail) :- - urdf_joint_hard_limits(pr2, l_wrist_roll_joint, _, _, _). - -test(joint_upper_limit_pr2_torso_lift_joint) :- - urdf_joint_hard_limits(pr2, torso_lift_joint, [_,0.33], _, _). - -test(joint_upper_limit_pr2_r_forearm_roll_joint, fail) :- - urdf_joint_hard_limits(pr2, r_forearm_roll_joint, _, _, _). - -test(joint_vel_limit_pr2_r_gripper_joint) :- - urdf_joint_hard_limits(pr2, r_gripper_joint, _, 0.2, _). - -test(joint_effort_limit_pr2_head_pan_joint) :- - urdf_joint_hard_limits(pr2, head_pan_joint, _, _, 6.0). - -test(joint_calibration_rising_pr2_torso_lift_joint, fail) :- - urdf_joint_calibration_rising(pr2, torso_lift_joint, _). - -test(joint_calibration_falling_pr2_fl_caster_rotation_joint, fail) :- - urdf_joint_calibration_falling(pr2, fl_caster_rotation_joint, _). - -test(joint_calibration_rising_pr2_fl_caster_rotation_joint) :- - urdf_joint_calibration_rising(pr2, fl_caster_rotation_joint, -0.785398163397). - -test(joint_calibration_falling_pr2_torso_lift_joint) :- - urdf_joint_calibration_falling(pr2, torso_lift_joint, Falling), - assert_equals(Falling,0.00475). - -test(joint_dynamics_damping_pr2_l_torso_lift_side_plate_joint, fail) :- - urdf_joint_damping(pr2, l_torso_lift_side_plate_joint, _). - -test(joint_dynamics_friction_pr2_l_torso_lift_side_plate_joint, fail) :- - urdf_joint_friction(pr2, l_torso_lift_side_plate_joint, _). - -test(joint_dynamics_damping_pr2_head_pan_joint) :- - urdf_joint_damping(pr2, head_pan_joint, 0.5). - -test(joint_dynamics_friction_pr2_head_pan_joint) :- - urdf_joint_friction(pr2, head_pan_joint, 0.0). - -test(joint_safety_lower_limit_pr2_l_upper_arm_joint, fail) :- - urdf_joint_soft_limits(pr2, l_upper_arm_joint, _, _, _). - -test(joint_safety_upper_limit_pr2_l_upper_arm_joint, fail) :- - urdf_joint_soft_limits(pr2, l_upper_arm_joint, _, _, _). - -test(joint_safety_kp_pr2_l_upper_arm_joint, fail) :- - urdf_joint_soft_limits(pr2, l_upper_arm_joint, _, _, _). - -test(joint_safety_kv_pr2_l_upper_arm_joint, fail) :- - urdf_joint_soft_limits(pr2, l_upper_arm_joint, _, _, _). - -test(joint_safety_lower_limit_pr2_l_elbow_flex_joint) :- - urdf_joint_soft_limits(pr2, l_elbow_flex_joint, [-2.1213,_], _, _). - -test(joint_safety_upper_limit_pr2_l_elbow_flex_joint) :- - urdf_joint_soft_limits(pr2, l_elbow_flex_joint, [_,-0.15], _, _). - -test(joint_safety_kp_pr2_l_elbow_flex_joint) :- - urdf_joint_soft_limits(pr2, l_elbow_flex_joint, _, 100.0, _). - -test(joint_safety_kv_pr2_l_elbow_flex_joint) :- - urdf_joint_soft_limits(pr2, l_elbow_flex_joint, _, _, 3.0). - -test(link_inertial_origin_pr2_l_gripper_led_frame, fail) :- - urdf_link_inertial(pr2, l_gripper_led_frame, _, _, _). - -test(link_inertial_mass_pr2_l_gripper_led_frame, fail) :- - urdf_link_inertial(pr2, l_gripper_led_frame, _, _, _). - -test(link_inertial_inertia_pr2_l_gripper_led_frame, fail) :- - urdf_link_inertial(pr2, l_gripper_led_frame, _, _, _). - -test(link_inertial_origin_pr2_l_elbow_flex_link) :- - urdf_link_inertial(pr2, l_elbow_flex_link, _, - [l_elbow_flex_link, [0.01014, 0.00032, -0.01211], [0.0, 0.0, 0.0, 1.0]], - _). - -test(link_inertial_mass_pr2_l_elbow_flex_link) :- - urdf_link_inertial(pr2, l_elbow_flex_link, _, _, 1.90327). - -test(link_inertial_mass_pr2_l_elbow_flex_link) :- - urdf_link_inertial(pr2, l_elbow_flex_link, - [0.00346541989, 0.00004066825, 0.00043171614, 0.00441606455, 0.00003968914, 0.00359156824], - _, _). - -test(link_num_visuals_pr2_r_gripper_led_frame, [fail]) :- - urdf_link_visual_shape(pr2, r_gripper_led_frame, _, _, _, _). - -test(link_visual_type_pr2_r_gripper_motor_accelerometer_link) :- - urdf_link_visual_shape(pr2, r_gripper_motor_accelerometer_link, Shape, _, _, _), - assert_equals(Shape,box(0.001,0.001,0.001)). - -test(link_visual_type_pr2_r_gripper_motor_slider_link) :- - urdf_link_visual_shape(pr2, r_gripper_motor_slider_link, cylinder(0.002, 0.025), _, _, _). - -test(link_visual_geometry_pr2_head_mount_kinect_ir_link) :- - urdf_link_visual_shape(pr2, head_mount_kinect_ir_link, sphere(0.0005), _, _, _). - -test(link_visual_geometry_pr2_head_mount_link) :- - urdf_link_visual_shape(pr2, head_mount_link, mesh(_, [0.001, 0.001, 0.001]), _, _, _). - -test(link_origin_pr2_r_gripper_motor_slider_link) :- - urdf_link_visual_shape(pr2, r_gripper_motor_slider_link, _, - [ r_gripper_motor_slider_link, - [0.0,0.0,0.0], - [0.7071080798594737, 0.0, 0.0, 0.7071054825112363] - ], _, _). - -test(link_collision_geometry_pr2_r_gripper_r_finger_link) :- - urdf_link_collision_shape(pr2, r_gripper_r_finger_link, - mesh("package://pr2_description/meshes/gripper_v0/l_finger.stl", [1.0, 1.0, 1.0]), - _). - -test(link_num_collisions_pr2_base_laser_link, [fail]) :- - urdf_link_collision_shape(pr2, base_laser_link, _, _). - -test(link_num_collision_foo_link) :- - catch(urdf_link_collision_shape(pr2, foo, _, _), urdf_error(Msg), true), - ground(Msg). - -test(link_collision_geometry_pr2_head_mount_prosilica_link) :- - urdf_link_collision_shape(pr2, head_mount_prosilica_link, sphere(0.0005), _). - -test(link_collision_geometry_pr2_fr_caster_r_wheel_link) :- - urdf_link_collision_shape(pr2, fr_caster_r_wheel_link, cylinder(0.074792, 0.034), _). - -test(link_collision_geometry_pr2_torso_lift_motor_screw_link) :- - urdf_link_collision_shape(pr2, torso_lift_motor_screw_link, box(0.5, 0.7, 0.01), _). - -test(link_collision_origin_pr2_r_gripper_r_finger_link) :- - urdf_link_collision_shape(pr2, r_gripper_r_finger_link, _, - [r_gripper_r_finger_link, [0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]]). - -test(urdf_unload) :- - urdf_unload_file(pr2). - -:- end_rdf_tests('ros_urdf'). diff --git a/src/urdf/__init__.pl b/src/urdf/__init__.pl deleted file mode 100644 index 472c9ec..0000000 --- a/src/urdf/__init__.pl +++ /dev/null @@ -1,2 +0,0 @@ - -:- use_module('URDF'). diff --git a/src/urdf/parser.cpp b/src/urdf/parser.cpp deleted file mode 100755 index 09596f0..0000000 --- a/src/urdf/parser.cpp +++ /dev/null @@ -1,506 +0,0 @@ -/* - Copyright (C) 2018 Georg Bartels - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#include -#include -#include -#include -#include -// SWI Prolog -#define PL_SAFE_ARG_MACROS -#include - -/**************************************/ -/********** CONVERSIONS ***************/ -/**************************************/ - -PlTerm to_prolog_pose(const std::string &frame, const urdf::Pose& p) { - // - PlTerm pos; - PlTail posl(pos); - posl.append(p.position.x); - posl.append(p.position.y); - posl.append(p.position.z); - posl.close(); - // - PlTerm rot; - PlTail rotl(rot); - rotl.append(p.rotation.x); - rotl.append(p.rotation.y); - rotl.append(p.rotation.z); - rotl.append(p.rotation.w); - rotl.close(); - // - PlTerm pose; - PlTail l(pose); - l.append(frame.c_str()); - l.append(pos); - l.append(rot); - l.close(); - return pose; -} - -PlCompound to_prolog_geometry(urdf::GeometryConstSharedPtr geom) { - if (!geom) - throw PlException(PlCompound("urdf_error", PlTerm("null_geometry"))); - - switch(geom->type) { - case urdf::Geometry::BOX: { - urdf::BoxConstSharedPtr box = urdf::dynamic_pointer_cast(geom); - PlTermv dim_term(3); - dim_term[0] = box->dim.x; - dim_term[1] = box->dim.y; - dim_term[2] = box->dim.z; - return PlCompound("box", dim_term); - } - case urdf::Geometry::SPHERE: { - urdf::SphereConstSharedPtr sphere = urdf::dynamic_pointer_cast(geom); - PlTermv dim_term(1); - dim_term[0] = sphere->radius; - return PlCompound("sphere", dim_term); - } - case urdf::Geometry::CYLINDER : { - urdf::CylinderConstSharedPtr cylinder = urdf::dynamic_pointer_cast(geom); - PlTermv dim_term(2); - dim_term[0] = cylinder->radius; - dim_term[1] = cylinder->length; - return PlCompound("cylinder", dim_term); - } - case urdf::Geometry::MESH : { - urdf::MeshConstSharedPtr mesh = urdf::dynamic_pointer_cast(geom); - PlTermv dim_term(2); - dim_term[0] = mesh->filename.c_str(); - PlTail scale(dim_term[1]); - scale.append(mesh->scale.x); - scale.append(mesh->scale.y); - scale.append(mesh->scale.z); - scale.close(); - return PlCompound("mesh", dim_term); - } - default: - throw PlException(PlCompound("urdf_error", - PlTerm("unsupported_geometry"))); - } -} - -PlTerm to_prolog_material(const urdf::MaterialSharedPtr& material) { - PlTermv mat_term(1); - PlTail args(mat_term[0]); - if(material) { - if(!material->texture_filename.empty()) { - PlTermv file_term(1); - file_term[0] = material->texture_filename.c_str(); - args.append(PlCompound("texture_file",file_term)); - } - // - PlTermv col_term(1); - PlTail col_list(col_term[0]); - col_list.append(material->color.r); - col_list.append(material->color.g); - col_list.append(material->color.b); - col_list.append(material->color.a); - col_list.close(); - args.append(PlCompound("rgba", col_term)); - } - args.close(); - return PlCompound("material", mat_term); -} - -/**************************************/ -/********** INIT URDF *****************/ -/**************************************/ - -std::map robot_models; -std::mutex robot_models_mtx; - -urdf::Model& get_robot_model(const char *id) { - std::unique_lock lock(robot_models_mtx); - return robot_models[std::string(id)]; -} - -urdf::LinkConstSharedPtr get_link(const char* urdf_id, const char* link_name) { - urdf::LinkConstSharedPtr link = get_robot_model(urdf_id).getLink(std::string(link_name)); - if (!link) - throw PlException(PlCompound("urdf_error", - PlCompound("no_such_link", PlTerm(link_name)))); - return link; -} - -urdf::JointConstSharedPtr get_joint(const char* urdf_id, const char* joint_name) { - urdf::JointConstSharedPtr joint = get_robot_model(urdf_id).getJoint(std::string(joint_name)); - if (!joint) - throw PlException(PlCompound("urdf_error", - PlCompound("no_such_joint", PlTerm(joint_name)))); - return joint; -} - -bool link_has_visual_with_index(const urdf::LinkConstSharedPtr link, long index) { - return link && (index >= 0) && (index < link->visual_array.size()) && - (link->visual_array[index]) && - //(link->visual_array[index]->name.compare("") != 0) && - (link->visual_array[index]->geometry); -} - -bool link_has_collision_with_index(const urdf::LinkConstSharedPtr link, long index) { - return link && (index >= 0) && (index < link->collision_array.size()) && - (link->collision_array[index]) && - //(link->collision_array[index]->name.compare("") != 0) && - (link->collision_array[index]->geometry); -} - -bool joint_has_pos_limits(urdf::JointConstSharedPtr joint) { - return joint->type == urdf::Joint::PRISMATIC || - joint->type == urdf::Joint::REVOLUTE; -} - -bool joint_has_vel_limit(urdf::JointConstSharedPtr joint) { - return joint->type == urdf::Joint::REVOLUTE || - joint->type == urdf::Joint::PRISMATIC || - joint->type == urdf::Joint::CONTINUOUS; -} - -bool joint_has_effort_limit(urdf::JointConstSharedPtr joint) { - // joints that have velocity limits should also have effort limits - return joint_has_vel_limit(joint); -} - -const char* get_joint_type(urdf::JointConstSharedPtr joint) { - switch (joint->type) { - case urdf::Joint::REVOLUTE: return "revolute"; - case urdf::Joint::PRISMATIC: return "prismatic"; - case urdf::Joint::CONTINUOUS: return "continuous"; - case urdf::Joint::FIXED: return "fixed"; - case urdf::Joint::PLANAR: return "planar"; - case urdf::Joint::FLOATING: return "floating"; - default: return "unknown"; - } -} - -/**************************************/ -/******** PROLOG PREDICATES ***********/ -/**************************************/ - -// urdf_load_file(Object, File) -PREDICATE(urdf_load_file, 2) { - std::string filename((char*)PL_A2); - if(get_robot_model(PL_A1).initFile(filename)) { - return true; - } else { - std::unique_lock lock(robot_models_mtx); - std::string urdf_id((char*)PL_A1); - robot_models.erase(urdf_id); - return false; - } -} - -// urdf_load_xml(Object, XML_data) -PREDICATE(urdf_load_xml, 2) { - std::string xml_data((char*)PL_A2); - if(get_robot_model(PL_A1).initString(xml_data)) { - return true; - } else { - std::unique_lock lock(robot_models_mtx); - std::string urdf_id((char*)PL_A1); - robot_models.erase(urdf_id); - return false; - } -} - -// urdf_is_loaded(Object) -PREDICATE(urdf_is_loaded,1) { - std::string urdf_id((char*)PL_A1); - std::unique_lock lock(robot_models_mtx); - return robot_models.find(urdf_id) != robot_models.end(); -} - -// urdf_unload_file(Object) -PREDICATE(urdf_unload_file, 1) { - std::string urdf_id((char*)PL_A1); - std::unique_lock lock(robot_models_mtx); - robot_models.erase(urdf_id); - return true; -} - -//PREDICATE(load_urdf_param, 2) { -// std::string urdf_id((char*)PL_A1); -// std::string param_name((char*)PL_A2); -// return get_robot_model(urdf_id).initParam(param_name); -//} - -//PREDICATE(load_urdf_string, 2) { -// std::string urdf_id((char*)PL_A1); -// std::string urdf_string((char*)PL_A2); -// return get_robot_model(urdf_id).initString(urdf_string); -//} - -// urdf_robot_name(Object, Name) -PREDICATE(urdf_robot_name, 2) { - PL_A2 = get_robot_model(PL_A1).name_.c_str(); - return true; -} - -// urdf_link_names(Object, LinkNames) -PREDICATE(urdf_link_names, 2) { - PlTail names(PL_A2); - for (auto const& link_entry: get_robot_model(PL_A1).links_) - names.append(link_entry.first.c_str()); - return names.close(); -} - -// urdf_joint_names(Object, JointNames) -PREDICATE(urdf_joint_names, 2) { - PlTail names(PL_A2); - for (auto const& joint_entry: get_robot_model(PL_A1).joints_) - names.append(joint_entry.first.c_str()); - return names.close(); -} - -// urdf_root_link(Object, RootLink) -PREDICATE(urdf_root_link, 2) { - PL_A2 = get_robot_model(PL_A1).root_link_->name.c_str(); - return true; -} - -// urdf_link_parent_joint(Object, Link, Parent) -PREDICATE(urdf_link_parent_joint, 3) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - if (link->parent_joint) { - PL_A3 = link->parent_joint->name.c_str(); - return true; - } - else { - return false; - } -} - -// urdf_link_child_joints(Object, Link, Children) -PREDICATE(urdf_link_child_joints, 3) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - PlTail child_joints(PL_A3); - for (auto const& child_joint: link->child_joints) { - child_joints.append(child_joint->name.c_str()); - } - return child_joints.close(); -} - -// urdf_link_inertial(Object, Link, Inertia, Origin, Mass) -PREDICATE(urdf_link_inertial, 5) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - if(link->inertial) { - PlTail inertia(PL_A3); - inertia.append(link->inertial->ixx); - inertia.append(link->inertial->ixy); - inertia.append(link->inertial->ixz); - inertia.append(link->inertial->iyy); - inertia.append(link->inertial->iyz); - inertia.append(link->inertial->izz); - inertia.close(); - PL_A4 = to_prolog_pose(link->name, link->inertial->origin); - PL_A5 = link->inertial->mass; - return true; - } - else { - return false; - } -} - -// urdf_link_num_visuals(Object,Link,Count) -PREDICATE(urdf_link_num_visuals, 3) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - PL_A3 = (long) link->visual_array.size(); - return true; -} - -// urdf_link_nth_visual_shape(Object,Link,Index,ShapeTerm,Origin,MaterialTerm) -PREDICATE(urdf_link_nth_visual_shape, 6) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - long index = (long) PL_A3; - if (link_has_visual_with_index(link, index)) { - //link->visual_array[index]->name.c_str(); - PL_A4 = to_prolog_geometry(link->visual_array[index]->geometry); - PL_A5 = to_prolog_pose(link->name,link->visual_array[index]->origin); - PL_A6 = to_prolog_material(link->visual_array[index]->material); - return true; - } - else { - return false; - } -} - -// urdf_link_num_collisions(Object,Link,Count) -PREDICATE(urdf_link_num_collisions, 3) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - PL_A3 = (long) link->collision_array.size(); - return true; -} - -// urdf_link_nth_collision_shape(Object,Link,Index,ShapeTerm,Origin) -PREDICATE(urdf_link_nth_collision_shape, 5) { - urdf::LinkConstSharedPtr link = get_link(PL_A1,PL_A2); - long index = (long) PL_A3; - if (link_has_collision_with_index(link, index)) { - //link->collision_array[index]->name.c_str(); - PL_A4 = to_prolog_geometry(link->collision_array[index]->geometry); - PL_A5 = to_prolog_pose(link->name,link->collision_array[index]->origin); - return true; - } - else { - return false; - } -} - -// urdf_joint_type(Object, Joint, Type) -PREDICATE(urdf_joint_type, 3) { - PL_A3 = get_joint_type(get_joint(PL_A1,PL_A2)); - return true; -} - -// urdf_joint_child_link(Object, Joint, Child) -PREDICATE(urdf_joint_child_link, 3) { - PL_A3 = get_joint(PL_A1,PL_A2)->child_link_name.c_str(); - return true; -} - -// urdf_joint_parent_link(Object, Joint, Parent) -PREDICATE(urdf_joint_parent_link, 3) { - PL_A3 = get_joint(PL_A1,PL_A2)->parent_link_name.c_str(); - return true; -} - -// urdf_joint_axis(Object, Joint, [X,Y,Z]) -PREDICATE(urdf_joint_axis, 3) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - // joint axis not defined for the following three joint types - if (joint->type == urdf::Joint::FIXED || - joint->type == urdf::Joint::UNKNOWN || - joint->type == urdf::Joint::FLOATING) { - return false; - } - PlTail l(PL_A3); - l.append(joint->axis.x); - l.append(joint->axis.y); - l.append(joint->axis.z); - return l.close(); -} - -// urdf_joint_origin(Object, Joint, [Frame,Position,Rotation]) -// This is the transform from the parent link to the child link. -// The joint is located at the origin of the child link -PREDICATE(urdf_joint_origin, 3) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - PL_A3 = to_prolog_pose( - joint->parent_link_name, - joint->parent_to_joint_origin_transform); - return true; -} - -// urdf_joint_calibration(Object, Joint, Falling) -PREDICATE(urdf_joint_calibration_falling, 3) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - if (joint->calibration && joint->calibration->falling) { - PL_A3 = *(joint->calibration->falling); - return true; - } - else { - return false; - } -} - -// urdf_joint_calibration(Object, Joint, Rising) -PREDICATE(urdf_joint_calibration_rising, 3) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - if (joint->calibration && joint->calibration->rising) { - PL_A3 = *(joint->calibration->rising); - return true; - } - else { - return false; - } -} - -// urdf_joint_damping(Object, Joint, Damping) -PREDICATE(urdf_joint_damping, 3) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - if (joint->dynamics){ - PL_A3 = joint->dynamics->damping; - return true; - } - else { - return false; - } -} - -// urdf_joint_friction(Object, Joint, Friction) -PREDICATE(urdf_joint_friction, 3) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - if (joint->dynamics){ - PL_A3 = joint->dynamics->friction; - return true; - } - else { - return false; - } -} - -// urdf_joint_hard_limits(Object, Joint, [LL,UL], VelMax, EffMax) -PREDICATE(urdf_joint_hard_limits, 5) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - if (joint->limits) { - PlTail l(PL_A3); - if(joint_has_pos_limits(joint)) { - l.append(joint->limits->lower); - l.append(joint->limits->upper); - } - else { - return false; - } - l.close(); - PL_A4 = (joint_has_vel_limit(joint) ? joint->limits->velocity : 0.0); - PL_A5 = (joint_has_effort_limit(joint) ? joint->limits->effort : 0.0); - return true; - } - else { - return false; - } -} - -// urdf_joint_soft_limits(Object, Joint, [LL,UL], KP, KV) -PREDICATE(urdf_joint_soft_limits, 5) { - urdf::JointConstSharedPtr joint = get_joint(PL_A1,PL_A2); - if (joint->safety) { - PlTail l(PL_A3); - l.append(joint->safety->soft_lower_limit); - l.append(joint->safety->soft_upper_limit); - l.close(); - PL_A4 = joint->safety->k_position; - PL_A5 = joint->safety->k_velocity; - return true; - } - else { - return false; - } -} diff --git a/src/url.pl b/src/url.pl deleted file mode 100755 index d2549ef..0000000 --- a/src/url.pl +++ /dev/null @@ -1,46 +0,0 @@ -:- module(utils_url, - [ ros_package_iri/2 - ]). -/** Resolving URLs to local filesystem. - -@author Daniel Beßler -@license BSD -*/ - -:- use_module('filesystem.pl', [ path_concat/3 ]). - -:- dynamic ros_package_iri_/2. - -%% -url_resolve(A,B) :- ros_resolve(A,B),!. - -%% -ros_resolve(PackagePath,LocalPath) :- - ros_path(PackagePath,LocalPath). - -ros_resolve(URL,LocalPath) :- - %% map IRI to ROS package path based on predicate *ros_package_iri_* - file_base_name(URL,FileName), - file_directory_name(URL,Prefix), - ros_package_iri_(Pkg,Prefix), - ros_package_path(Pkg,PkgPath), - %% convention is that rdf files are stored in a directory named "owl" or "rdf" - ( path_concat(PkgPath,owl,Dir); - path_concat(PkgPath,'owl/external',Dir); - path_concat(PkgPath,rdf,Dir); - Dir=PkgPath - ), - path_concat(Dir,FileName,LocalPath), - exists_file(LocalPath), - !. - -%% ros_package_iri(+PkgName,+URI) is det. -% -% Register an IRI for a ROS package. -% When RDF files are loaded with the IRI prefix, -% it is first tried to serve the local file from -% the ROS package before downoading the file -% from the web. -% -ros_package_iri(PkgName,URI) :- - assertz(ros_package_iri_(PkgName,URI)). diff --git a/src/vis/README.md b/src/vis/README.md deleted file mode 100755 index e8220b6..0000000 --- a/src/vis/README.md +++ /dev/null @@ -1,22 +0,0 @@ -\page ros_vis Marker Visualization - -Utilities for the visualization of knowledge pieces. -This package follows a client-server scheme: -The server side (knowrob_vis) generates only messages describing what -should be visualized, and clients can subscribe to these messages -and update their canvas accordingly. - -One of the supported clients is [RViz](http://wiki.ros.org/rviz) -which uses so called *Marker* messages to tell RViz what should -be displayed. -These describe some 3D visual information, such as some -basic 3D shapes, but also complex meshes. - -The second visualization interface implemented in this package is the data visualization -module data_vis.pl. -[Data visualization messages](https://raw.githubusercontent.com/code-iai/iai_common_msgs/master/data_vis_msgs/msg/DataVis.msg) contain -a set of data records together with some visualization hints such -as diagram type, axis labels, font size, etc. -How in particular the data is displayed is up to the data visualization -client. -At the moment only [openEASE](http://www.open-ease.org/) implements such a client. diff --git a/src/vis/__init__.pl b/src/vis/__init__.pl deleted file mode 100644 index ed743ec..0000000 --- a/src/vis/__init__.pl +++ /dev/null @@ -1 +0,0 @@ -:- use_module('./data_vis.pl'). diff --git a/src/vis/data_vis.pl b/src/vis/data_vis.pl deleted file mode 100755 index a764c4c..0000000 --- a/src/vis/data_vis.pl +++ /dev/null @@ -1,215 +0,0 @@ -/* - Copyright (C) 2015 Moritz Tenorth - Copyright (C) 2017 Daniel Beßler - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -:- module(data_vis, - [ - data_vis/2, - data_vis_remove/1, - timeline/1, - timeline_data/1, - timeline_data/2 - ]). -/** Methods for data visualization - - The visualization artifacts are supposed to be generated client side. - KnowRob only generates a data structure holding the data and - data visualization clients can connect to the topic on which - KnowRob publishes the visualization data. - - @author Moritz Tenorth - @author Daniel Beßler - @license BSD -*/ -:- use_module(library('semweb/rdfs')). -:- use_module(library('semweb/rdf_db')). - -%% data_vis(+Term:term, +Properties:list) is det -% -% Creates a new data_vis message and publishes it via _|/data_vis_msgs|_ -% ROS topic. -% Term is one of piechart(Identifier), barchart(Identifier), -% treechart(Identifier), timeline(Identifier), or linechart(Identifier). -% Properties is a list of properties for the data_vis message of the form key:value. -% Allowed keys are: data, title, xlabel, ylabel, width, height, fontsize. -% -% data_vis(piechart(chart_id), [ -% title: 'Some Distribution', -% data: [[a,b,c],[10,30,22]] -% ]). -% -% @param Term the data_vis term -% @param Properties list of data_vis properties -% -data_vis(Term, Properties) :- - data_vis_object(Term, Object), !, - data_vis_set_properties(Object, Properties), - data_vis_publish(Object). - -%% timeline(+Events:list) is det -% -% Creates a new data_vis timeline message and publishes it via _|/data_vis_msgs|_ -% ROS topic. -% -% @param Events list of temporal extended things -% -timeline(Events) :- - findall(EvtName, ( - member(Evt,Events), - once(( - is_classified_by(Evt, Task), - rdf_split_url(_,EvtName,Task))), - time_interval_data(Evt, _,_) - ), EvtNames), - findall(Time, ( - member(Evt, Events), - once(( - time_interval_data(Evt, T0,T1), - atomic_list_concat([T0,T1], '_', Time))) - ), EventExtends), - data_vis(timeline(event_timeline), - [values:[EvtNames,EventExtends]]). - -%% timeline_data(+Events:list) is det -% -% Creates a new data_vis timeline message and publishes it via _|/data_vis_msgs|_ -% ROS topic. -% -% @param EventData list of list of the form [[Events, Task, Start, End]] -% -timeline_data(EventsData) :- - timeline_data(EventsData,[]). - -timeline_data(EventsData,Options) :- - findall(EvtName, ( - member([_,Task,_,_],EventsData), - once(( - rdf_split_url(_,EvtName,Task))) - ), EvtNames), - findall(Time, ( - member([_,_,Start,End], EventsData), - atomic_list_concat([Start, End],'_',Time) - ), EventExtends), - data_vis(timeline(event_timeline), - [values:[EvtNames,EventExtends] | Options]). - -data_vis_set_(Key,Msg,Value) :- - get_dict(Key,Msg,[Type,_]), - b_set_dict(Key,Msg,[Type,Value]). - -%% data_vis_object -data_vis_object(piechart(Identifier), Object) :- - data_vis_(Identifier,0,Object). -data_vis_object(barchart(Identifier), Object) :- - data_vis_(Identifier,1,Object). -data_vis_object(treechart(Identifier), Object) :- - data_vis_(Identifier,2,Object). -data_vis_object(timeline(Identifier), Object) :- - data_vis_(Identifier,3,Object). -data_vis_object(linechart(Identifier), Object) :- - data_vis_(Identifier,4,Object). -data_vis_object(table(Identifier), Object) :- - data_vis_(Identifier,100,Object). -data_vis_object(graph(Identifier), Object) :- - data_vis_(Identifier,999,Object). - -data_vis_object(type(Identifier,TypeID), Object) :- - data_vis_(Identifier,TypeID,Object). - -data_vis_(Identitier, Type, _{ - id: [string,Identitier], - type: [int32,Type], - title: [string,''], - xlabel: [string,''], - ylabel: [string,''], - width: [int32,200], - height: [int32,200], - fontsize: [string,'12px'], - values: ['array(data_vis_msgs/ValueList)',[]] -}). - -%% data_vis_set_data -data_vis_set_data(Object, Data) :- - data_vis_set_property(Object, values:Data). - -%% data_vis_set_property -data_vis_set_property(Object, data:Data) :- - data_vis_set_property(Object, values:Data),!. -data_vis_set_property(Object, values:Data) :- - data_vis_values(Data,Values),!, - data_vis_set_(values, Object, [Values]). -data_vis_set_property(Object, array_data:Array) :- - !, - findall(Y, - ( member(X,Array), - data_vis_values(X,Y) - ), - ValuesArray - ), - data_vis_set_(values, Object, ValuesArray). -data_vis_set_property(Object, Key:Value) :- - data_vis_set_(Key, Object, Value). - -%% data_vis_set_properties -data_vis_set_properties(_, []) :- !. -data_vis_set_properties(Object, [Prop|Rest]) :- - data_vis_set_property(Object, Prop), !, - data_vis_set_properties(Object, Rest). - -%% -data_vis_values([Data1,Data2],_{ - label: [string,''], - value1: ['array(string)',D1], - value2: ['array(string)',D2] -}) :- - string_data_(Data1,D1), - string_data_(Data2,D2),!. -data_vis_values(Data1,_{ - label: [string,''], - value1: ['array(string)',D1], - value2: ['array(string)',[]] -}) :- - string_data_(Data1,D1),!. - -%% -string_data_([],[]) :- !. -string_data_([X|Xs],[Y|Ys]) :- - ( atom(X) -> Y=X ; term_to_atom(X,Y) ), - string_data_(Xs,Ys). - -%% -data_vis_publish(Object) :- - ros_publish('/data_vis_msgs', 'data_vis_msgs/DataVis', Object). - -%% data_vis_remove(+Identifier) is det. -% -% Republishes a data_vis object with empty data -% so that visualization clients can remove it. -% -data_vis_remove(Identifier) :- - data_vis_(Identifier,0,Object), - data_vis_publish(Object). diff --git a/src/vis/vis.pl b/src/vis/vis.pl deleted file mode 100755 index 4291bf3..0000000 --- a/src/vis/vis.pl +++ /dev/null @@ -1,110 +0,0 @@ -/* - Copyright (C) 2015 Daniel Beßler - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY - DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -:- module(knowrob_vis, []). - -%:- module(knowrob_vis, -% [ -% show/1, -% show/2, -% hide/1 -% ]). -%/** Methods for visualizing parts of the knowledge base -% -% @author Daniel Beßler -% @license BSD -%*/ -%:- use_module(library('semweb/rdfs')). -%:- use_module(library('semweb/rdf_db')). -%:- use_module(library('knowrob/data_vis')). -% -%:- rdf_meta -% hide(t), -% show(t), -% show(t,t), -% hide_marker(t), -% show_marker(t,t). -% -%:- multifile show/2, -% hide/1, -% show_marker/2, -% hide_marker/1. -% -%%% show(+Thing) is det. -%%% show(+Thing, +Properties) is det. -%% -%% This is a non-logical predicate used to invoke -%% external clients such us RViz -%% to visualize objects in the knowledge base. -%% Custom visualization back-ends may be used -%% by defining another clause of this multifile -%% predicate. -%% -%show(Things) :- -% is_list(Things), !, -% show_next_, -% forall( member(Thing, Things), ( -% T =.. [show|Thing], call(T) -% )), !. -%show(Thing) :- -% show(Thing,[]). -%show(Thing, Properties) :- -% show_marker(Thing,Properties). -% -%%% show_next is det -%show_next_ :- -% true. -% -%%% hide(+Thing) is det. -%% -%% This is a non-logical predicate used to invoke -%% external clients such us RViz -%% to remove visualizations of objects. -%% -%hide(Things) :- -% is_list(Things), !, -% forall( member(Thing, Things), ( -% T =.. [hide|Thing], call(T) -% )), !. -%hide(Thing) :- -% hide_marker(Thing). -% -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % -%% % % % % Marker visualization -% -%%% -%show_marker(Thing, Properties) :- -% atom(Thing), -% rdf_resource(Thing), -% object_state(Thing, State, Properties), -% object_state_add_cpp([State]). -%%% -%hide_marker(Thing) :- -% atom(Thing), -% rdf_resource(Thing), -% object_state_remove_cpp([Thing]).