diff --git a/.github/workflows/main.yml b/.github/workflows/overworld.yml
similarity index 89%
rename from .github/workflows/main.yml
rename to .github/workflows/overworld.yml
index 8948783b..998ac678 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/overworld.yml
@@ -1,4 +1,4 @@
-name: ROS CI
+name: overworld CI
on: [push]
@@ -34,7 +34,7 @@ jobs:
sudo apt install dpkg
sudo apt install -y ros-$ROS_DISTRO-desktop qtbase5-dev qtdeclarative5-dev
sudo apt install -y libcppunit-dev
- sudo apt install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-tf2-geometry-msgs ros-$ROS_DISTRO-pr2-common
+ sudo apt install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-tf2-geometry-msgs
if [ $ROS_DISTRO != noetic ]
then
sudo apt-get install python-rosdep
@@ -64,13 +64,7 @@ jobs:
make install
cd ~/work
git clone -b dev https://github.com/sarthou/ontologenius.git
- git clone https://github.com/RIS-WITH/optitrack_msgs.git
- if [ $ROS_DISTRO != noetic ]
- then
- git clone https://github.com/sarthou/ar_track_alvar.git
- else
- git clone -b noetic-devel https://github.com/sarthou/ar_track_alvar.git
- fi
+
- name: build
run: |
diff --git a/.github/workflows/overworld_modules.yml b/.github/workflows/overworld_modules.yml
new file mode 100644
index 00000000..3b318b45
--- /dev/null
+++ b/.github/workflows/overworld_modules.yml
@@ -0,0 +1,98 @@
+name: overworld modules CI
+
+on: [push]
+
+jobs:
+ build:
+
+ runs-on: ${{ matrix.operating-system }}
+ strategy:
+ matrix:
+ operating-system: [ubuntu-18.04]
+ include:
+ - operating-system: ubuntu-18.04
+ distro: melodic
+ - operating-system: ubuntu-20.04
+ distro: noetic
+ env:
+ ROS_CI_DESKTOP: "`lsb_release -cs`"
+ CI_SOURCE_PATH: $(pwd)
+ ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall
+ CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options
+ ROS_PARALLEL_JOBS: '-j8 -l6'
+ # Set the python path manually to include /usr/-/python2.7/dist-packages
+ # as this is where apt-get installs python packages.
+ PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages:/usr/lib/python3.8/dist-packages:/usr/local/lib/python3.8/dist-packages
+ ROS_DISTRO: ${{ matrix.distro }}
+ steps:
+ - uses: actions/checkout@v1
+ - name: Install ROS
+ run: |
+ sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+ sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+ sudo apt update -qq
+ sudo apt install dpkg
+ sudo apt install -y ros-$ROS_DISTRO-desktop qtbase5-dev qtdeclarative5-dev
+ sudo apt install -y libcppunit-dev
+ sudo apt install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-tf2-geometry-msgs ros-$ROS_DISTRO-pr2-common
+ if [ $ROS_DISTRO != noetic ]
+ then
+ sudo apt-get install python-rosdep
+ else
+ sudo apt-get install python3-rosdep
+ fi
+
+ source /opt/ros/$ROS_DISTRO/setup.bash
+ sudo rosdep init
+ rosdep update --include-eol-distros # Support EOL distros.
+ if [ $ROS_DISTRO != noetic ]
+ then
+ pip install requests --user
+ else
+ pip3 install requests --user
+ fi
+
+ - name: Install dependencies
+ run: |
+ mkdir -p ~/dependencies && cd ~/dependencies
+ git clone https://github.com/bulletphysics/bullet3.git
+ cd bullet3
+ mkdir build && mkdir install
+ export BULLET_PATH=$(pwd)
+ cd build
+ cmake .. -DCMAKE_INSTALL_PREFIX=$BULLET_PATH/install -DBUILD_SHARED_LIBS=ON
+ make install
+ cd ~/work
+ git clone -b dev https://github.com/sarthou/ontologenius.git
+
+
+ - name: Install modules
+ run: |
+ cd ~/work
+ mkdir overworld_modules && cd overworld_modules
+ git clone https://github.com/sarthou/artrack_perception_module.git
+ git clone https://github.com/sarthou/optitrack_perception_module.git
+ git clone https://github.com/sarthou/pr2_gripper_perception_module.git
+ cd ..
+ git clone https://github.com/RIS-WITH/optitrack_msgs.git
+ if [ $ROS_DISTRO != noetic ]
+ then
+ git clone https://github.com/sarthou/ar_track_alvar.git
+ else
+ git clone -b noetic-devel https://github.com/sarthou/ar_track_alvar.git
+ fi
+
+
+ - name: build
+ run: |
+ source /opt/ros/$ROS_DISTRO/setup.bash
+ mkdir -p ~/catkin_ws/src
+ cd ~/catkin_ws
+ catkin_make
+ source devel/setup.bash
+ cd ~/catkin_ws/src
+ ln -s ~/work
+ cd ..
+ export BULLET_INSTALL_PATH=~/dependencies/bullet3/install
+ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$BULLET_INSTALL_PATH/lib
+ catkin_make
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ebb52d0..43de3db7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,11 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
tf2_geometry_msgs
roslib
message_filters
- ar_track_alvar_msgs
sensor_msgs
visualization_msgs
geometry_msgs
- optitrack_msgs
cv_bridge
pluginlib
)
@@ -66,6 +64,9 @@ endforeach()
add_message_files(
FILES
Triplet.msg
+ EntityPose.msg
+ EntitiesPoses.msg
+ AgentPose.msg
)
add_service_files(
@@ -92,9 +93,9 @@ generate_messages(
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
-# INCLUDE_DIRS include
-# LIBRARIES overworld
- CATKIN_DEPENDS ontologenius roscpp std_msgs tf2 tf2_ros roslib
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}_bullet_lib ${PROJECT_NAME}_types_lib ${PROJECT_NAME}_utility_lib
+ CATKIN_DEPENDS ontologenius roscpp std_msgs tf2 tf2_ros roslib
# DEPENDS system_lib
)
@@ -141,6 +142,13 @@ target_link_libraries(${PROJECT_NAME}_types_lib PUBLIC
${PROJECT_NAME}_bullet_lib)
set_target_properties(${PROJECT_NAME}_types_lib PROPERTIES POSITION_INDEPENDENT_CODE ON)
+add_library(${PROJECT_NAME}_utility_lib STATIC
+ src/Utility/YamlReader.cpp
+)
+target_link_libraries(${PROJECT_NAME}_utility_lib PUBLIC
+ ${catkin_LIBRARIES})
+set_target_properties(${PROJECT_NAME}_utility_lib PROPERTIES POSITION_INDEPENDENT_CODE ON)
+
add_library(${PROJECT_NAME}_perception_lib STATIC
src/Perception/Managers/ObjectsPerceptionManager.cpp
src/Perception/Managers/RobotsPerceptionManager.cpp
@@ -148,12 +156,12 @@ add_library(${PROJECT_NAME}_perception_lib STATIC
src/Perception/Modules/ObjectsModules/ObjectsEmulatedPerceptionModule.cpp
src/Perception/Modules/HumansModules/HumansEmulatedPerceptionModule.cpp
src/Perception/PerceptionManager.cpp
- src/Perception/PerceptionConfiguration.cpp
)
target_link_libraries(${PROJECT_NAME}_perception_lib PUBLIC
${catkin_LIBRARIES}
${PROJECT_NAME}_bullet_lib
- ${PROJECT_NAME}_types_lib)
+ ${PROJECT_NAME}_types_lib
+ ${PROJECT_NAME}_utility_lib)
add_library(${PROJECT_NAME}_facts_lib STATIC
src/Facts/Publisher/FactsPublisher.cpp
@@ -169,6 +177,7 @@ add_library(${PROJECT_NAME}_sender_lib STATIC
src/Senders/ROSSender.cpp
src/Senders/PoseSender.cpp
src/Senders/Bernie.cpp
+ src/Senders/RelationsSender.cpp
)
target_link_libraries(${PROJECT_NAME}_sender_lib PUBLIC
${catkin_LIBRARIES}
@@ -181,21 +190,23 @@ target_link_libraries(${PROJECT_NAME}_sender_lib PUBLIC
#################
add_library(overworld_modules_plugin MODULE
- src/Perception/Modules/ObjectsModules/ArTrackPerceptionModule.cpp
src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp
- src/Perception/Modules/ObjectsModules/Pr2GripperPerceptionModule.cpp
- src/Perception/Modules/RobotsModules/PR2JointsPerception.cpp
- src/Perception/Modules/HumansModules/OptitrackPerceptionModule.cpp
+ src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp
+ src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp
+ src/Perception/Modules/RobotsModules/FakeRobotPerceptionModule.cpp
+ src/Perception/Modules/HumansModules/StampedPosePerceptionModule.cpp
+ src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp
)
target_link_libraries(overworld_modules_plugin PUBLIC
${catkin_LIBRARIES}
${PROJECT_NAME}_bullet_lib
- ${PROJECT_NAME}_types_lib)
+ ${PROJECT_NAME}_types_lib
+ ${PROJECT_NAME}_utility_lib)
add_library(overworld_reasoner_plugin
src/OntologeniusPlugins/ReasonerEgocentric.cpp
)
-add_dependencies(overworld_reasoner_plugin overworld_gencpp)
+add_dependencies(overworld_reasoner_plugin overworld_gencpp ${catkin_EXPORTED_TARGETS})
target_link_libraries(overworld_reasoner_plugin PUBLIC
${catkin_LIBRARIES}
${ontologenius_LIBRARIES})
@@ -212,6 +223,10 @@ target_link_libraries(${PROJECT_NAME}_node PRIVATE
${catkin_LIBRARIES})
add_dependencies(${PROJECT_NAME}_node overworld_gencpp)
+add_executable(plugins src/Nodes/plugins.cpp)
+target_link_libraries(plugins ${catkin_LIBRARIES}
+ ${PROJECT_NAME}_bullet_lib)
+
#################
# Test files #
#################
diff --git a/Readme.md b/Readme.md
index 4813654f..e76e2ca3 100644
--- a/Readme.md
+++ b/Readme.md
@@ -71,8 +71,8 @@ The knowledge base Ontologenius can be used as a source of information for Overw
More information about this link and tutorials to show its use will come soon.
-[Release-Url]: https://github.com/sarthou/overworld/releases/tag/v0.1.2
-[Release-Image]: http://img.shields.io/badge/release-v0.1.2-blue
+[Release-Url]: https://github.com/sarthou/overworld/releases/tag/v0.1.3
+[Release-Image]: http://img.shields.io/badge/release-v0.1.3-blue
[Ontologenius-Dependency-Image]: https://img.shields.io/badge/dependencies-ontologenius-yellow
[Ontologenius-Dependency-Url]: https://github.com/sarthou/ontologenius
diff --git a/config/adream.yaml b/config/adream.yaml
new file mode 100644
index 00000000..775470fc
--- /dev/null
+++ b/config/adream.yaml
@@ -0,0 +1,93 @@
+adream_walls:
+ orientation:
+ z: 1.57
+
+adream_floor:
+ orientation:
+ z: 1.57
+
+adream_window_h10_1:
+ orientation:
+ z: 1.57
+
+adream_window_h10_2:
+ orientation:
+ z: 1.57
+
+adream_window_h20_1:
+ orientation:
+ z: 1.57
+
+adream_window_h20_2:
+ orientation:
+ z: 1.57
+
+adream_window_h20_3:
+ orientation:
+ z: 1.57
+
+adream_appartment:
+ orientation:
+ z: 1.57
+
+adream_door_h21_1:
+ position:
+ x: 1.03963
+ y: -0.00339
+ z: 1.0725
+ orientation:
+ z: 1.57
+
+adream_door_h21_2:
+ position:
+ x: 5.09035
+ y: 18.4801
+ z: 1.0725
+ orientation:
+ z: 1.57
+
+adream_door_h21_3:
+ position:
+ x: 0.932577
+ y: 18.4514
+ z: 1.0725
+ orientation:
+ z: 1.57
+
+adream_door_h16:
+ position:
+ x: -0.152311
+ y: 22.3506
+ z: 1.0725
+ orientation:
+ z: 3.14159
+
+adream_door_h12:
+ position:
+ x: -11.5018
+ y: 11.6701
+ z: 1.0725
+ orientation:
+ z: 2.51327
+
+adream_door_h14:
+ position:
+ x: -6.56428
+ y: 9.44934
+ z: 1.0725
+
+adream_door_h13:
+ position:
+ x: -10.236
+ y: 11.2895
+ z: 1.0725
+ orientation:
+ z: -0.942478
+
+adream_door_h11:
+ position:
+ x: -9.51151
+ y: 14.0825
+ z: 1.0725
+ orientation:
+ z: -0.942478
diff --git a/config/config_example.yaml b/config/config_example.yaml
index 400cc5b4..e2c06f69 100644
--- a/config/config_example.yaml
+++ b/config/config_example.yaml
@@ -1,6 +1,6 @@
modules:
robot:
- PR2JointsPerception: pr2_joints
+ JointStatePerceptionModule: robot_joints
humans:
OptitrackPerceptionModule: optitrack
objects:
@@ -10,8 +10,8 @@ modules:
- pr2_right_gripper
StaticObjectsPerceptionModule: static
-pr2_joints:
- name : pr2_robot
+robot_joints:
+ name : pr2
right_hand : r_gripper_tool_frame
left_hand : l_gripper_tool_frame
head : head_mount_kinect2_rgb_optical_frame
@@ -28,4 +28,7 @@ pr2_right_gripper:
optitrack:
name: human_0
offset_x: 6.4868
- offset_y: 2.8506
\ No newline at end of file
+ offset_y: 2.8506
+
+static:
+ file: package://overworld/config/adream.yaml
\ No newline at end of file
diff --git a/docs/AvailableModules.html b/docs/AvailableModules.html
new file mode 100644
index 00000000..4a14124e
--- /dev/null
+++ b/docs/AvailableModules.html
@@ -0,0 +1,152 @@
+
+
+
+ Available Modules | Overworld 0.1.3
+
+
+
+
+
Overworld Documentation
+
+
+
+
+
+
+
Available modules
+
+
With Overworld, a perception module is an entry point to gather data either about objects, humans, or the robot. To be as modular
+ as possible and to adapt to your specific needs, Overworld uses a plugin system for its perception modules.
+
+
Here you have a list and description of built-in modules and additional known modules.
+
+
Built-in modules
+
+
+
+
A human perception module providing fake body parts from a ROS topic.
+
+
+
+
A robot perception module providing fake body parts from a ROS topic.
+
+
+
+
An object perception module providing fake objects from a ROS topic.
+
+
+
+
A robot perception module using robot joint state and robot description file (urdf).
+
+
+
+
A human head perception module using a poseStamped ROS message.
+ The module subscribe to "/peopleTrack".
+
+
+
+
An object perception module providing static objects.
+
+
External modules
+
+
Packages providing the external modules just have to be cloned in your ROS workspace.
+
+
+
+
An object perception module using the Ar_track_alvar system.
+
+
+
+
A human perception module using the Optitrack mocap system.
+
+
+
+
An object perception module using the Pr2 pressure sensors.
+
+
+
+
+
Download | Overworld 0.1.2
+ Download | Overworld 0.1.3
Getting Start
@@ -60,8 +60,8 @@ Overview
@@ -89,6 +89,56 @@ Download
You have below the archives of the major versions of Overworld:
+ V0.1.3
+ 14 / 12 / 2022
+
+
+
+ V0.1.2
+ 7 / 10 / 2022
+
+
+
V0.1.1
15 / 02 / 2022
diff --git a/docs/InstallOverworld.html b/docs/InstallOverworld.html
index 7183764b..fdfd9c2b 100644
--- a/docs/InstallOverworld.html
+++ b/docs/InstallOverworld.html
@@ -1,7 +1,7 @@
- Install | Overworld 0.1.2
+ Install | Overworld 0.1.3
Getting Start
@@ -70,8 +70,8 @@ Overview
diff --git a/docs/Publications.html b/docs/Publications.html
index 0cbd1c12..5f28b6f0 100644
--- a/docs/Publications.html
+++ b/docs/Publications.html
@@ -1,7 +1,7 @@
- Publications | Overworld 0.1.2
+ Publications | Overworld 0.1.3
Getting Start
@@ -60,8 +60,8 @@ Overview
diff --git a/docs/construction.html b/docs/construction.html
index 7369c121..3c2a054d 100644
--- a/docs/construction.html
+++ b/docs/construction.html
@@ -50,8 +50,8 @@ Getting Start
@@ -62,8 +62,8 @@ Overview
diff --git a/docs/index.html b/docs/index.html
index 2a0efe25..4d82345b 100644
--- a/docs/index.html
+++ b/docs/index.html
@@ -1,7 +1,7 @@
- Overworld 0.1.2
+ Overworld 0.1.3
Assessing the geometry of the world for Human-Robot Interaction
+
@@ -101,7 +102,7 @@ Assessing the geometry of the world for Human-Robot Interaction
@@ -158,13 +159,23 @@
About overworld
What's New
+
+
+
Tune your Overworld
+
+
Overworld now comes with more parameters allowing you to adjust its setting to your specific use.
+
You want to desactivate the physics simulation? You want to change the assessment frequency? Now you can!
+
+
+
+
Simulation-based reasoning
Overworld now fully uses the Bullet physics simulator implementing a new simulation-based reasoning process
to estimate the position of objects which are no more perceived.
-
You have dropped an object in a box? Overworld understand it !
+
You have dropped an object in a box? Overworld understand it!
diff --git a/docs/overview/Configuration.html b/docs/overview/Configuration.html
index 3feb1d3c..88eb160f 100644
--- a/docs/overview/Configuration.html
+++ b/docs/overview/Configuration.html
@@ -1,7 +1,7 @@
-
Configuration | Overworld 0.1.2
+
Configuration | Overworld 0.1.3
@@ -48,8 +48,8 @@
Getting Start
@@ -60,8 +60,8 @@
Overview
@@ -91,7 +91,7 @@ Configuration
modules :
robot :
-
PR2JointsPerception : pr2_joints
+
JointStatePerceptionModule : pr2_joints
humans :
OptitrackPerceptionModule : optitrack
objects :
@@ -120,15 +120,21 @@
Configuration
name : human_0
offset_x : 6.4868
offset_y : 2.8506
+
+
static :
+
file : package://overworld/config/adream.yaml
To pass your configuration file to an Overworld executable see the
launcher page
+ To see the currently installed modules and their available parameters take a look to the
+ plugins page
+
The modules correspond to the perception modules that will be loaded at the launch. These modules are divided in three categories: robot , humans , and objects .
- In each category, the keys correspond to the modules' type while the values are the user-defined instances names. For example, in the provided configuration file, we load a PR2JointsPerception module called pr2_joints and two Pr2GripperPerceptionModule modules called r_gripper_tool_frame and pr2_right_gripper
+ In each category, the keys correspond to the modules' type while the values are the user-defined instances names. For example, in the provided configuration file, we load a JointStatePerceptionModule module called pr2_joints and two Pr2GripperPerceptionModule modules called r_gripper_tool_frame and pr2_right_gripper
Plugins | Overworld 0.1.3
+
+
+
+
+
Overworld Documentation
+
+
+
+
+
+
+
Plugins
+
+
Within Overworld, perception modules are implemented in the form of plugins. Modules to be used and their parameters are defined in the Overworld configuration file .
+
+
Overworld comes with an executable to list the currently installed modules, get their description and parameters, or get the package implementing them. Its usage is the following:
+
+
+
rosrun overworld plugins COMMAND [ARGS]
+
+
+
+
+
+
rosrun overworld plugins list [<option>]
+
+
+
This command lists the available perception modules in the form of plugins.
+
+
The option can either be Objects or BodyParts to focus on specific module type.
+
+
+
+
+
rosrun overworld plugins description <module>
+
+
+
Given a module name, this command provides its description.
+
+
the module argument is the name of an existing perception module.
+
+
+
+
+
rosrun overworld plugins library_path <module>
+
+
+
Given a module name, this command provides the path to its associated library.
+
+
the module argument is the name of an existing perception module.
+
+
+
+
+
rosrun overworld plugins package <module>
+
+
+
Given a module name, this command provides the name of the containing package.
+
+
the module argument is the name of an existing perception module.
+
+
+
+
+
Bullet GUI | Overworld 0.1.2
+ Bullet GUI | Overworld 0.1.3
Getting Start
@@ -60,8 +60,8 @@ Overview
diff --git a/docs/overview/launchers.html b/docs/overview/launchers.html
index 4f6d6219..336b950e 100644
--- a/docs/overview/launchers.html
+++ b/docs/overview/launchers.html
@@ -1,7 +1,7 @@
- Launchers | Overworld 0.1.2
+ Launchers | Overworld 0.1.3
Getting Start
@@ -60,8 +60,8 @@ Overview
@@ -85,7 +85,7 @@
Launchers
- Overworld is quite easy to launch as it consists of only one executable and only needs two arguments. However, Overworld uses the Ontologenius knowledge base. Here we explain the minimal setup to launch both software.
+ Overworld is quite easy to launch as it consists of only one executable and only has two mandotory arguments. However, Overworld uses the Ontologenius knowledge base. Here we explain the minimal setup to launch both software.
<launch >
<arg name ="configuration_file" default ="$(find overworld)/config/config_example.yaml" />
<arg name ="robot_name" default ="pr2" />
- <node name ="overworld" pkg ="overworld" type ="overworld_node" output ="screen" args ="$(arg configuration_file) $(arg robot_name)" > </node >
+ <node name ="overworld" pkg ="overworld" type ="overworld_node" output ="screen" args ="-c $(arg configuration_file) -n $(arg robot_name)" > </node >
</launch >
- The configuration_file parameter is used to defined the perception modules to be loaded and to parametrized them. See the configuration page to setup your file.
+ The configuration_file (-c or --config) parameter is used to defined the perception modules to be loaded and to parametrized them. See the configuration page to setup your file.
- The robot_name parameter is (quite logically) the identifier of your robot. This parameter should be the same as the one defined in the configuration file.
+ The robot_name (-n or --name) parameter is (quite logically) the identifier of your robot. This parameter should be the same as the one defined in the configuration file.
+
+ The simulate (-s or --simulate) is an optional boolean ("true" or "false") parameter. It can be set to activate or deactivate the physics simulation in the reasoning process. By default, this parameter is set to true.
+
+ The assessment frequency (-af or --assessment-frequency) is an optional double parameter. It can be set to change the assessment loop frequency. By default, this parameter is set to 17hz.
+
+ The simulation frequency (-sf or --simulation-frequency) is an optional double parameter. It can be set to change the physical simulation frequency. By default, this parameter is set to 70hz.
& bb) { bounding_box_ = bb; }
const std::array& getBoundingBox() const { return bounding_box_; }
double getBbVolume() const;
+ void setOriginOffset(const std::array& origin_offset) { origin_offset_ = origin_offset; }
+ const std::array& getOriginOffset() const { return origin_offset_; }
+ void computeCorners();
+ std::vector getCorners() const { return corners_; }
+
+ double getMinDistanceTo(const Object& other);
void setMass(double mass) { mass_ = mass; }
void setDefaultMass(double density = 500.0); // shape and aabb should exist to compute the default mass
double getMass() const { return mass_; }
+ void setTypes(const std::vector& types);
+ bool isA(const std::string& type);
+
protected:
std::vector points_of_interest_;
bool is_static_;
Hand* hand_in_;
+
std::array bounding_box_;
+ std::array origin_offset_;
+ std::vector corners_;
+
double mass_;
+
+ std::unordered_set types_;
};
} // namespace owds
diff --git a/include/overworld/Bullet/BulletClient.h b/include/overworld/Bullet/BulletClient.h
index 7647fa50..edc9c22a 100644
--- a/include/overworld/Bullet/BulletClient.h
+++ b/include/overworld/Bullet/BulletClient.h
@@ -195,6 +195,10 @@ class BulletClient
std::mutex mutex_;
std::string additional_path_;
+ std::unordered_map loaded_textures_;
+ std::unordered_map loaded_collision_meshes_;
+ // Visual meshes are not cached as same texture would be
+ // applied to all same visual meshes
int createVisualShape(BulletShapeType_e shape_type,
float radius,
diff --git a/include/overworld/Facts/Fact.h b/include/overworld/Facts/Fact.h
index 1e7ffcb4..d74a3b3a 100644
--- a/include/overworld/Facts/Fact.h
+++ b/include/overworld/Facts/Fact.h
@@ -28,6 +28,11 @@ class Fact
return hash_ == other.hash_;
}
+ bool operator<(const Fact& other) const
+ {
+ return hash_ < other.hash_;
+ }
+
std::string toString(const std::string& delim = " ") const
{
return subject_ + delim + predicate_ + delim + object_;
@@ -42,4 +47,16 @@ class Fact
} // namespace owds
+namespace std {
+
+template <>
+struct hash
+{
+ std::size_t operator()(const owds::Fact& fact) const
+ {
+ return fact.getHash();
+ }
+};
+} // namespace std
+
#endif // OWDS_FACT_H
\ No newline at end of file
diff --git a/include/overworld/Facts/FactsCalculator.h b/include/overworld/Facts/FactsCalculator.h
index 9bfe1c23..714db55c 100644
--- a/include/overworld/Facts/FactsCalculator.h
+++ b/include/overworld/Facts/FactsCalculator.h
@@ -8,23 +8,23 @@
#include "overworld/BasicTypes/Object.h"
#include "overworld/BasicTypes/Agent.h"
#include "overworld/Facts/Fact.h"
-#include
namespace owds {
class FactsCalculator
{
public:
- FactsCalculator(ros::NodeHandle* nh, const std::string& agent_name);
- std::vector computeFacts(const std::map& objects,
- const std::map& agents,
- const std::map>& segmantation_ids);
+ explicit FactsCalculator(const std::string& agent_name);
+
+ std::vector computeObjectsFacts(const std::map& objects,
+ bool clear = true);
+ std::vector computeAgentsFacts(const std::map& objects,
+ const std::map& agents,
+ const std::map>& segmantation_ids,
+ bool clear = true);
private:
std::vector facts_;
- OntologiesManipulator ontos_;
- OntologyManipulator* onto_;
-
bool isOnTopfOf(Object* object_under, Object* object_on);
bool isInContainer(Object* object_around, Object* object_in);
@@ -36,6 +36,8 @@ class FactsCalculator
bool isLookingAt(Agent* agent, const std::unordered_set& seen_bullet_ids, const Object* object);
bool hasInHand(Agent* agent, Object* object);
bool isHandMovingTowards(Agent* agent, Object* object);
+
+ bool isValid(Object* object);
};
} // namespace owds
diff --git a/include/overworld/Facts/Publisher/FactsPublisher.h b/include/overworld/Facts/Publisher/FactsPublisher.h
index 8c11d098..b223539b 100644
--- a/include/overworld/Facts/Publisher/FactsPublisher.h
+++ b/include/overworld/Facts/Publisher/FactsPublisher.h
@@ -3,9 +3,10 @@
#include "overworld/Facts/Fact.h"
-#include
+#include
#include
#include
+#include
namespace owds {
@@ -21,8 +22,14 @@ class FactsPublisher
virtual void removeFromKb(const Fact& fact) = 0;
private:
- std::map pending_facts_;
+ std::unordered_map pending_facts_;
std::unordered_set held_by_;
+
+ std::unordered_map facts_buffer_;
+ std::unordered_map rmv_buffer_;
+
+ std::vector filterIncomingFacts(const std::vector& facts);
+ std::unordered_set filterOutgoingFacts(const std::unordered_set& facts);
};
} // namespace owds
diff --git a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h
index cbff3b7f..ef6eea43 100644
--- a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h
+++ b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h
@@ -3,6 +3,8 @@
#include "ontologenius/core/reasoner/plugins/ReasonerInterface.h"
+#include "overworld/GetRelations.h"
+
namespace ontologenius {
class ReasonerEgocentric : public ReasonerInterface
@@ -21,10 +23,16 @@ class ReasonerEgocentric : public ReasonerInterface
virtual std::string getDesciption() override;
private:
std::unordered_set computable_properties_;
+ ros::NodeHandle n_;
+ ros::ServiceClient overworld_client_;
ObjectPropertyBranch_t* isComputableProperty(const std::string& property);
std::set isInRange(const std::string& indiv, ObjectPropertyBranch_t* property);
std::set isInDomain(const std::string& indiv, ObjectPropertyBranch_t* property);
+
+ overworld::GetRelations createRequest(const std::string& subject, const std::set& predicates, const std::string& object);
+ bool call(overworld::GetRelations& srv);
+ void updateOntology(const std::vector& to_add, const std::vector& to_delete);
};
} // namespace ontologenius
diff --git a/include/overworld/Perception/Managers/EntitiesPerceptionManager.h b/include/overworld/Perception/Managers/EntitiesPerceptionManager.h
index 5d42b098..fba49e17 100644
--- a/include/overworld/Perception/Managers/EntitiesPerceptionManager.h
+++ b/include/overworld/Perception/Managers/EntitiesPerceptionManager.h
@@ -86,8 +86,7 @@ template
std::vector EntitiesPerceptionManager::getModulesList() const
{
std::vector modules_name;
- for(const auto& module : perception_modules_)
- modules_name.push_back(module.first);
+ std::transform(perception_modules_.cbegin(), perception_modules_.cend(), std::back_inserter(modules_name), [](const auto& it){return it.first;});
return modules_name;
}
@@ -146,10 +145,7 @@ void EntitiesPerceptionManager::deleteModules()
template
bool EntitiesPerceptionManager::shouldRun()
{
- for(const auto& module : perception_modules_)
- if(module.second->isActivated() && module.second->hasBeenUpdated())
- return true;
- return false;
+ return std::any_of(perception_modules_.begin(), perception_modules_.end(), [](const auto& it){return it.second->isActivated() && it.second->hasBeenUpdated();});
}
template
diff --git a/include/overworld/Perception/Managers/ObjectsPerceptionManager.h b/include/overworld/Perception/Managers/ObjectsPerceptionManager.h
index 0ab5e130..ee136b1e 100644
--- a/include/overworld/Perception/Managers/ObjectsPerceptionManager.h
+++ b/include/overworld/Perception/Managers/ObjectsPerceptionManager.h
@@ -5,13 +5,15 @@
#include "overworld/BasicTypes/Agent.h"
#include "overworld/BasicTypes/Object.h"
+#include
+
namespace owds {
class ObjectsPerceptionManager : public EntitiesPerceptionManager
{
public:
- inline ObjectsPerceptionManager(): EntitiesPerceptionManager(), myself_agent_(nullptr){}
- void setOwnerAgent(Agent* agent) { myself_agent_ = agent; }
+ explicit ObjectsPerceptionManager(ros::NodeHandle* nh): EntitiesPerceptionManager(), myself_agent_(nullptr), ontos_(OntologiesManipulator(nh)), onto_(nullptr){}
+ void setOwnerAgent(Agent* agent);
std::map getEntities() const;
@@ -30,6 +32,9 @@ class ObjectsPerceptionManager : public EntitiesPerceptionManager
std::unordered_set false_ids_to_be_merged_;
std::map merged_ids_;
+ OntologiesManipulator ontos_;
+ OntologyManipulator* onto_;
+
std::map::iterator createFromPercept(const Object& percept);
void getPercepts(std::map& percepts) override;
diff --git a/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h b/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h
new file mode 100644
index 00000000..8825f44e
--- /dev/null
+++ b/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h
@@ -0,0 +1,38 @@
+#ifndef OWDS_FAKEHUMANPERCEPTIONMODULE_H
+#define OWDS_FAKEHUMANPERCEPTIONMODULE_H
+
+#include "overworld/BasicTypes/BodyPart.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
+
+#include "overworld/AgentPose.h"
+
+#include "ontologenius/OntologiesManipulator.h"
+
+#include
+#include
+
+namespace owds {
+
+
+class FakeHumanPerceptionModule : public PerceptionModuleRosBase
+{
+ public:
+ FakeHumanPerceptionModule();
+
+ virtual bool closeInitialization() override;
+
+ private:
+ bool perceptionCallback(const overworld::AgentPose& msg);
+
+ BodyPart createBodyPart(const std::string& human_name, const std::string& part_name);
+
+ OntologiesManipulator* ontologies_manipulator_;
+ OntologyManipulator* onto_;
+
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf2_listener_;
+};
+
+} // namespace owds
+
+#endif // OWDS_FAKEHUMANPERCEPTIONMODULE_H
diff --git a/include/overworld/Perception/Modules/HumansModules/HumansEmulatedPerceptionModule.h b/include/overworld/Perception/Modules/HumansModules/HumansEmulatedPerceptionModule.h
index 3018c50b..835bc537 100644
--- a/include/overworld/Perception/Modules/HumansModules/HumansEmulatedPerceptionModule.h
+++ b/include/overworld/Perception/Modules/HumansModules/HumansEmulatedPerceptionModule.h
@@ -14,7 +14,7 @@ class HumansEmulatedPerceptionModule : public PerceptionModuleBase& msg);
+ bool perceptionCallback(const std::vector& msg) override;
std::map::iterator createNewPercept(BodyPart* object);
};
diff --git a/include/overworld/Perception/Modules/HumansModules/OptitrackPerceptionModule.h b/include/overworld/Perception/Modules/HumansModules/OptitrackPerceptionModule.h
deleted file mode 100644
index 91179dae..00000000
--- a/include/overworld/Perception/Modules/HumansModules/OptitrackPerceptionModule.h
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef OWDS_OPTITRACKPERCEPTIONMODULE_H
-#define OWDS_OPTITRACKPERCEPTIONMODULE_H
-
-#include "overworld/BasicTypes/BodyPart.h"
-#include "overworld/Perception/Modules/PerceptionModuleBase.h"
-
-#include
-
-#include "ontologenius/OntologiesManipulator.h"
-
-namespace owds {
-
-typedef std::pair BodyPartOptitrackPose;
-
-class OptitrackPerceptionModule : public PerceptionModuleBase_
-{
- public:
- OptitrackPerceptionModule();
-
- virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) override;
- virtual bool closeInitialization() override;
-
- private:
- void headRosCallback(const optitrack_msgs::or_pose_estimator_state& msg);
- void leftHandRosCallback(const optitrack_msgs::or_pose_estimator_state& msg);
- void rightHandRosCallback(const optitrack_msgs::or_pose_estimator_state& msg);
-
- void privatePerceptionCallback(const BodyPartOptitrackPose& msg);
-
- protected:
- bool perceptionCallback(const BodyPartOptitrackPose& msg);
-
- OntologiesManipulator* ontologies_manipulator_;
- OntologyManipulator* onto_;
- double offset_x_;
- double offset_y_;
- double offset_z_;
- Pose mocap_offset_;
-
- std::string human_name_;
- std::string head_name_, left_hand_name_, right_hand_name_;
-
- ros::Subscriber optitrack_head_sub_;
- ros::Subscriber optitrack_left_hand_sub_;
- ros::Subscriber optitrack_right_hand_sub_;
-};
-
-} // namespace owds
-
-#endif /* OWDS_OPTITRACKPERCEPTIONMODULE_H */
diff --git a/include/overworld/Perception/Modules/HumansModules/StampedPosePerceptionModule.h b/include/overworld/Perception/Modules/HumansModules/StampedPosePerceptionModule.h
new file mode 100644
index 00000000..e92941df
--- /dev/null
+++ b/include/overworld/Perception/Modules/HumansModules/StampedPosePerceptionModule.h
@@ -0,0 +1,34 @@
+#ifndef OWDS_STAMPEDPOSEPERCEPTIONMODULE_H
+#define OWDS_STAMPEDPOSEPERCEPTIONMODULE_H
+
+#include "overworld/BasicTypes/BodyPart.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
+
+#include
+
+#include "ontologenius/OntologiesManipulator.h"
+
+namespace owds {
+
+
+class StampedPosePerceptionModule : public PerceptionModuleRosBase
+{
+ public:
+ StampedPosePerceptionModule();
+
+ virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) override;
+ virtual bool closeInitialization() override;
+
+ protected:
+ bool perceptionCallback(const geometry_msgs::PoseStamped& msg) override;
+
+ OntologiesManipulator* ontologies_manipulator_;
+ OntologyManipulator* onto_;
+
+ std::string human_name_;
+ std::string head_name_;
+};
+
+} // namespace owds
+
+#endif /* OWDS_STAMPEDPOSEPERCEPTIONMODULE_H */
diff --git a/include/overworld/Perception/Modules/ObjectsModules/ArTrackPerceptionModule.h b/include/overworld/Perception/Modules/ObjectsModules/ArTrackPerceptionModule.h
deleted file mode 100644
index 34ce6279..00000000
--- a/include/overworld/Perception/Modules/ObjectsModules/ArTrackPerceptionModule.h
+++ /dev/null
@@ -1,56 +0,0 @@
-#ifndef OWDS_ARTRACKPERCEPTIONMODULE_H
-#define OWDS_ARTRACKPERCEPTIONMODULE_H
-
-#include "overworld/BasicTypes/Object.h"
-#include "overworld/BasicTypes/Agent.h"
-#include "overworld/Perception/Modules/PerceptionModuleBase.h"
-
-#include "ar_track_alvar_msgs/AlvarMarkers.h"
-#include "ar_track_alvar_msgs/AlvarVisibleMarkers.h"
-
-#include "ontologenius/OntologiesManipulator.h"
-
-#include
-#include
-
-#include
-#include
-
-namespace owds {
-
-class ArTrackPerceptionModule : public PerceptionModuleRosSyncBase
-{
-public:
- ArTrackPerceptionModule();
-
- virtual bool closeInitialization() override;
-
-private:
- Pose last_head_pose_;
-
- std::map ids_map_;
- std::unordered_set blacklist_ids_;
- std::unordered_set visible_markers_with_pois_; // The id of the visible markers that we already seen at least once and were valid (and thus, have their pois created)
-
- OntologiesManipulator* ontologies_manipulator_;
- OntologyManipulator* onto_;
-
- tf2_ros::Buffer tf_buffer_;
- tf2_ros::TransformListener tf2_listener_;
-
- bool perceptionCallback(const ar_track_alvar_msgs::AlvarMarkers& markers,
- const ar_track_alvar_msgs::AlvarVisibleMarkers& visible_markers) override;
-
- bool headHasMoved();
- bool isInValidArea(const Pose& tag_pose);
-
- void setPointOfInterest(const ar_track_alvar_msgs::AlvarVisibleMarker& visible_marker);
- void setAllPoiUnseen();
- void updateEntities(const ar_track_alvar_msgs::AlvarMarkers& main_markers,
- const std::unordered_set& invalid_main_markers_ids);
- bool createNewEntity(const ar_track_alvar_msgs::AlvarMarker& marker);
-};
-
-} // namespace owds
-
-#endif // OWDS_ARTRACKPERCEPTIONMODULE_H
\ No newline at end of file
diff --git a/include/overworld/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.h b/include/overworld/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.h
new file mode 100644
index 00000000..a2a73608
--- /dev/null
+++ b/include/overworld/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.h
@@ -0,0 +1,35 @@
+#ifndef OWDS_FAKEOBJECTPERCEPTIONMODULE_H
+#define OWDS_FAKEOBJECTPERCEPTIONMODULE_H
+
+#include "overworld/BasicTypes/Object.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
+
+#include "overworld/EntitiesPoses.h"
+
+#include
+#include
+
+namespace owds {
+
+class FakeObjectPerceptionModule : public PerceptionModuleRosBase
+{
+public:
+ FakeObjectPerceptionModule();
+
+ bool closeInitialization() override;
+
+private:
+ OntologiesManipulator* ontologies_manipulator_;
+ OntologyManipulator* onto_;
+
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf2_listener_;
+
+ virtual bool perceptionCallback(const overworld::EntitiesPoses& msg) override;
+
+ Object createNewEntity(const std::string& id);
+};
+
+} // namespace owds
+
+#endif // OWDS_FAKEOBJECTPERCEPTIONMODULE_H
\ No newline at end of file
diff --git a/include/overworld/Perception/Modules/ObjectsModules/Pr2GripperPerceptionModule.h b/include/overworld/Perception/Modules/ObjectsModules/Pr2GripperPerceptionModule.h
deleted file mode 100644
index 06e1698a..00000000
--- a/include/overworld/Perception/Modules/ObjectsModules/Pr2GripperPerceptionModule.h
+++ /dev/null
@@ -1,54 +0,0 @@
-#ifndef OWDS_PR2GRIPPERPERCEPTIONMODULE_H
-#define OWDS_PR2GRIPPERPERCEPTIONMODULE_H
-
-#include "overworld/BasicTypes/Object.h"
-#include "overworld/BasicTypes/Agent.h"
-#include "overworld/Bullet/BulletClient.h"
-#include "overworld/Perception/Modules/PerceptionModuleBase.h"
-
-#include "pr2_msgs/PressureState.h"
-
-namespace owds {
-
-enum Pr2GripperSide_e
-{
- PR2_GRIPPER_LEFT,
- PR2_GRIPPER_RIGHT,
- PR2_GRIPER_UNDEFINED
-};
-
-class Pr2GripperPerceptionModule : public PerceptionModuleRosBase
-{
-public:
- Pr2GripperPerceptionModule();
-
- virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) override;
- virtual bool closeInitialization() override;
-
-private:
- Pr2GripperSide_e side_;
- int pr2_left_tip_bullet_id_;
- int pr2_right_tip_bullet_id_;
-
- bool is_init_;
- double left_tip_pressure_;
- double left_tip_pressure_prev_;
- double right_tip_pressure_;
- double right_tip_pressure_prev_;
-
- bool has_picked_;
- size_t obj_id_;
- std::string current_obj_id_;
-
- double pressure_threshold_;
- double distance_threshold_;
- double min_period_;
- ros::Time last_update_;
-
- virtual bool perceptionCallback(const pr2_msgs::PressureState& msg) override;
- double getGripperDistance();
-};
-
-} // namespace owds
-
-#endif // OWDS_PR2GRIPPERPERCEPTIONMODULE_H
\ No newline at end of file
diff --git a/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h b/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h
index 7f825b00..6b0c03dc 100644
--- a/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h
+++ b/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h
@@ -13,12 +13,15 @@ class StaticObjectsPerceptionModule : public PerceptionModuleBase_
public:
StaticObjectsPerceptionModule();
+ virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) override;
virtual bool closeInitialization() override;
private:
OntologiesManipulator* ontologies_manipulator_;
OntologyManipulator* onto_;
+ std::string config_file_;
+
void addObject(const std::string& name,
const std::string& visual_mesh,
const std::string& colision_mesh,
diff --git a/include/overworld/Perception/Modules/PerceptionModuleBase.h b/include/overworld/Perception/Modules/PerceptionModuleBase.h
index 83f69f01..491d7461 100644
--- a/include/overworld/Perception/Modules/PerceptionModuleBase.h
+++ b/include/overworld/Perception/Modules/PerceptionModuleBase.h
@@ -29,7 +29,7 @@ class PerceptionModuleBase_
public:
explicit PerceptionModuleBase_(bool need_access_to_external_entities = false):
is_activated_(true),
- updated_(false),
+ updated_(true),
need_access_to_external_entities_(need_access_to_external_entities),
n_(nullptr),
bullet_client_(nullptr),
@@ -39,11 +39,13 @@ class PerceptionModuleBase_
{}
virtual ~PerceptionModuleBase_() = default;
- virtual void initialize(ros::NodeHandle* n,
+ virtual void initialize(const std::string& module_name,
+ ros::NodeHandle* n,
BulletClient* bullet_client,
int robot_bullet_id,
Agent* robot_agent)
{
+ module_name_ = module_name;
n_ = n;
bullet_client_ = bullet_client;
robot_bullet_id_ = robot_bullet_id;
@@ -51,7 +53,7 @@ class PerceptionModuleBase_
}
virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) {}
- virtual bool closeInitialization() { return true; }
+ virtual bool closeInitialization() { updated_ = false; return true; }
virtual std::string getAgentName() { return ""; }
virtual int getAgentBulletId() { return -1; }
@@ -132,6 +134,8 @@ class PerceptionModuleBase_
}
protected:
+ std::string module_name_;
+
std::map percepts_;
std::atomic is_activated_;
std::atomic updated_;
@@ -201,16 +205,17 @@ class PerceptionModuleRosBase : public PerceptionModuleBase_
{}
virtual ~PerceptionModuleRosBase() = default;
- virtual void initialize(ros::NodeHandle* n,
+ virtual void initialize(const std::string& module_name,
+ ros::NodeHandle* n,
BulletClient* bullet_client,
int robot_bullet_id,
Agent* robot_agent) override
{
- PerceptionModuleBase_::initialize(n, bullet_client, robot_bullet_id, robot_agent);
+ PerceptionModuleBase_::initialize(module_name, n, bullet_client, robot_bullet_id, robot_agent);
if(topic_name_ != "")
{
sub_ = this->n_->subscribe(topic_name_, 1, &PerceptionModuleRosBase::privatePerceptionCallback, this);
- ShellDisplay::info("PerceptionModuleRosBase subscribed to " + topic_name_);
+ ShellDisplay::info("[" + this->module_name_ + "] subscribed to " + topic_name_);
}
}
@@ -223,7 +228,7 @@ class PerceptionModuleRosBase : public PerceptionModuleBase_
if(topic_name_ != "")
{
sub_ = this->n_->subscribe(topic_name_, 1, &PerceptionModuleRosBase::privatePerceptionCallback, this);
- ShellDisplay::info("PerceptionModuleRosBase subscribed to " + topic_name_);
+ ShellDisplay::info("[" + this->module_name_ + "] subscribed to " + topic_name_);
}
}
@@ -268,17 +273,18 @@ class PerceptionModuleRosSyncBase : public PerceptionModuleBase_
{}
virtual ~PerceptionModuleRosSyncBase() = default;
- virtual void initialize(ros::NodeHandle* n,
+ virtual void initialize(const std::string& module_name,
+ ros::NodeHandle* n,
BulletClient* bullet_client,
int robot_bullet_id,
Agent* robot_agent) override
{
- PerceptionModuleBase_::initialize(n, bullet_client, robot_bullet_id, robot_agent);
+ PerceptionModuleBase_::initialize(module_name, n, bullet_client, robot_bullet_id, robot_agent);
sub_0_.subscribe(*n, first_topic_name_, 1);
sub_1_.subscribe(*n, second_topic_name_, 1);
sync_.reset(new Sync(SyncPolicy(10), sub_0_, sub_1_));
sync_->registerCallback(&PerceptionModuleRosSyncBase::privatePerceptionCallback, this);
- ShellDisplay::info("PerceptionModuleRosSyncBase subscribed to " + first_topic_name_ + " and " + second_topic_name_);
+ ShellDisplay::info("[" + this->module_name_ + "] subscribed to " + first_topic_name_ + " and " + second_topic_name_);
}
protected:
diff --git a/include/overworld/Perception/Modules/RobotsModules/FakeRobotPerceptionModule.h b/include/overworld/Perception/Modules/RobotsModules/FakeRobotPerceptionModule.h
new file mode 100644
index 00000000..781d7d11
--- /dev/null
+++ b/include/overworld/Perception/Modules/RobotsModules/FakeRobotPerceptionModule.h
@@ -0,0 +1,47 @@
+#ifndef OWDS_FAKEROBOTPERCEPTIONMODULE_H
+#define OWDS_FAKEROBOTPERCEPTIONMODULE_H
+
+#include "overworld/BasicTypes/BodyPart.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
+
+#include "overworld/AgentPose.h"
+
+#include
+#include
+
+namespace owds {
+
+class FakeRobotPerceptionModule : public owds::PerceptionModuleRosBase
+{
+ public:
+ FakeRobotPerceptionModule();
+ virtual ~FakeRobotPerceptionModule() = default;
+
+ virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) override;
+ virtual bool closeInitialization() override;
+
+ virtual std::string getAgentName() override { return robot_name_; }
+
+ protected:
+ bool perceptionCallback(const overworld::AgentPose& msg) override;
+ std::string robot_name_;
+
+ std::string head_link_;
+ std::string base_link_;
+
+ Pose head_pose_;
+ Pose base_pose_;
+
+ OntologiesManipulator* ontologies_manipulator_;
+ OntologyManipulator* onto_;
+
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf2_listener_;
+
+ BodyPart createBodyPart(const std::string& robot_name, const std::string& part_name, BodyPartType_e part_type);
+ Pose stringToPose(const std::string& str);
+};
+
+} // namespace owds
+
+#endif /* OWDS_FAKEROBOTPERCEPTIONMODULE_H */
diff --git a/include/overworld/Perception/Modules/RobotsModules/PR2JointsPerception.h b/include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h
similarity index 78%
rename from include/overworld/Perception/Modules/RobotsModules/PR2JointsPerception.h
rename to include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h
index a82180ff..3f94460d 100644
--- a/include/overworld/Perception/Modules/RobotsModules/PR2JointsPerception.h
+++ b/include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h
@@ -1,5 +1,5 @@
-#ifndef OWDS_PR2JOINTSPERCEPTION_H
-#define OWDS_PR2JOINTSPERCEPTION_H
+#ifndef OWDS_JOINTSTATEPERCEPTIONMODULE_H
+#define OWDS_JOINTSTATEPERCEPTIONMODULE_H
#include "overworld/BasicTypes/BodyPart.h"
#include "overworld/Bullet/PhysicsServers.h"
@@ -9,11 +9,11 @@
namespace owds {
-class PR2JointsPerception : public owds::PerceptionModuleRosBase
+class JointStatePerceptionModule : public owds::PerceptionModuleRosBase
{
public:
- PR2JointsPerception();
- virtual ~PR2JointsPerception() = default;
+ JointStatePerceptionModule();
+ virtual ~JointStatePerceptionModule() = default;
virtual void setParameter(const std::string& parameter_name, const std::string& parameter_value) override;
virtual bool closeInitialization() override;
@@ -47,4 +47,4 @@ class PR2JointsPerception : public owds::PerceptionModuleRosBase
-#include
-#include
-#include
-
-namespace owds
-{
-
-class PerceptionConfiguration;
-
-class ConfigElement
-{
- friend PerceptionConfiguration;
-public:
- ConfigElement operator[](const std::string& name)
- {
- if(subelem)
- {
- if(subelem.value().find(name) != subelem.value().end())
- return subelem.value()[name];
- else
- return ConfigElement();
- }
- else
- return ConfigElement();
- }
-
- std::vector value()
- {
- if(data)
- return data.value();
- else
- return {};
- }
-
- std::vector getElementsKeys()
- {
- std::vector res;
- if(subelem)
- for(auto& elem : subelem.value())
- res.push_back(elem.first);
- return res;
- }
-
-private:
- std::experimental::optional> data;
- std::experimental::optional> subelem;
-};
-
-class PerceptionConfiguration
-{
-public:
- bool read(const std::string& path);
-
- void display();
-
- ConfigElement operator[](const std::string& name)
- {
- if(elements_.find(name) != elements_.end())
- return elements_[name];
- else
- return ConfigElement();
- }
-
-private:
- std::map elements_;
-
- std::map read(const std::vector& lines, size_t& current_line);
- ConfigElement readList(const std::vector& lines, size_t& current_line);
-
- void display(std::map& config, size_t nb = 0);
- void displayTab(size_t nb);
- void removeComment(std::string& line);
- size_t countSpaces(const std::string& line);
-};
-
-} // namespace owds
-
-#endif // OWDS_PERCEPTIONCONFIGURATION_H
diff --git a/include/overworld/Perception/PerceptionManager.h b/include/overworld/Perception/PerceptionManager.h
index 93467935..b9da453a 100644
--- a/include/overworld/Perception/PerceptionManager.h
+++ b/include/overworld/Perception/PerceptionManager.h
@@ -5,7 +5,7 @@
#include "overworld/Perception/Managers/ObjectsPerceptionManager.h"
#include "overworld/Perception/Managers/HumansPerceptionManager.h"
-#include "overworld/Perception/PerceptionConfiguration.h"
+#include "overworld/Utility/YamlReader.h"
namespace owds {
@@ -41,9 +41,9 @@ class PerceptionManager
int robot_bullet_id_;
Agent* robot_agent_;
- bool applyConfiguration(const std::string& config_path, ConfigElement& modules_list);
+ bool applyConfiguration(const std::string& config_path, YamlElement& modules_list, bool display = true);
- PerceptionConfiguration configuration_;
+ YamlReader configuration_;
};
} // namespace owds
diff --git a/include/overworld/Senders/ROSSender.h b/include/overworld/Senders/ROSSender.h
index 8637368a..b22eff11 100644
--- a/include/overworld/Senders/ROSSender.h
+++ b/include/overworld/Senders/ROSSender.h
@@ -58,7 +58,7 @@ void ROSSender::sendEntitiesToRViz(const std::string& topic_name, const std::vec
for (const auto& entity : entities)
{
if (entity->isLocated())
- markers.markers.push_back(entity->toMarker(id++, 1.0, "marker"));
+ markers.markers.push_back(entity->toMarker(id++, 0.2, "marker"));
}
pub_it->second.publish(markers);
}
@@ -78,8 +78,8 @@ void ROSSender::sendEntitiesToTFAndRViz(const std::string& rviz_topic_name, cons
{
if (entity->isLocated())
{
- markers.markers.push_back(entity->toMarker(id++, 1.0, "marker"));
- transforms.push_back(entity->toTfTransform());
+ markers.markers.emplace_back(entity->toMarker(id++, 0.2, "marker"));
+ transforms.emplace_back(entity->toTfTransform());
}
}
pub_it->second.publish(markers);
@@ -112,7 +112,7 @@ void ROSSender::sendEntitiesToRViz(const std::string& topic_name, const std::map
for (const auto& entity : entities)
{
if (entity.second->isLocated())
- markers.markers.push_back(entity.second->toMarker(id++, 1.0, "marker"));
+ markers.markers.emplace_back(entity.second->toMarker(id++, 0.2, "marker"));
}
pub_it->second.publish(markers);
}
@@ -132,8 +132,8 @@ void ROSSender::sendEntitiesToTFAndRViz(const std::string& rviz_topic_name, cons
{
if (entity.second->isLocated())
{
- markers.markers.push_back(entity.second->toMarker(id++, 1.0, "marker"));
- transforms.push_back(entity.second->toTfTransform());
+ markers.markers.emplace_back(entity.second->toMarker(id++, 0.2, "marker"));
+ transforms.emplace_back(entity.second->toTfTransform());
}
}
pub_it->second.publish(markers);
diff --git a/include/overworld/Senders/RelationsSender.h b/include/overworld/Senders/RelationsSender.h
new file mode 100644
index 00000000..805770ff
--- /dev/null
+++ b/include/overworld/Senders/RelationsSender.h
@@ -0,0 +1,84 @@
+#ifndef OVERWORLD_RELATIONSSENDER_H
+#define OVERWORLD_RELATIONSSENDER_H
+
+#include "overworld/Perception/Managers/ObjectsPerceptionManager.h"
+#include "overworld/Perception/Managers/HumansPerceptionManager.h"
+#include "overworld/Perception/Managers/RobotsPerceptionManager.h"
+#include "overworld/Facts/Fact.h"
+
+#include "overworld/GetRelations.h"
+
+#include
+
+namespace owds {
+
+struct ToCompute_t
+{
+ ToCompute_t(bool egocentric = false) : deitic_relation(false),
+ intrinsic_relation(false),
+ egocentric_relation(egocentric) {}
+ bool deitic_relation;
+ bool intrinsic_relation;
+ bool egocentric_relation;
+ std::string subject;
+};
+
+struct ComputedRelation_t
+{
+ ComputedRelation_t() : last_complete_use(0) {}
+ uint64_t last_complete_use;
+ std::map last_indiv_use;
+};
+
+struct ComputedRelations_t
+{
+ ComputedRelations_t() : last_use(0) {}
+ uint64_t last_use;
+ ComputedRelation_t computed_egocentric_relation;
+ ComputedRelation_t computed_deictic_relation;
+ ComputedRelation_t computed_intrinsic_relation;
+};
+
+class RelationsSender
+{
+public:
+ RelationsSender(ros::NodeHandle* nh,
+ const std::string& agent_name,
+ ObjectsPerceptionManager* objects_manager,
+ HumansPerceptionManager* humans_manager,
+ RobotsPerceptionManager* robots_manager);
+
+ void posesUpdated() { frames_++; }
+
+private:
+ uint64_t frames_;
+ std::unordered_map last_use_;
+ std::unordered_map last_facts_;
+
+ ros::ServiceServer get_relations_service_;
+
+ std::string agent_name_;
+ ObjectsPerceptionManager* objects_manager_;
+ HumansPerceptionManager* humans_manager_;
+ RobotsPerceptionManager* robots_manager_;
+ BodyPart* agent_; // the agent is represented by one of its body part
+
+ void getAgent();
+
+ bool onGetRelationService(overworld::GetRelations::Request& request,
+ overworld::GetRelations::Response& response);
+ std::vector shouldRecompute(const overworld::GetRelations::Request& request);
+ bool shouldRecompute(const std::string& subject, ComputedRelation_t& computed_relation);
+
+ void computeRelationOnAll(const overworld::Triplet& pattern, overworld::GetRelations::Response& response, bool deictic, bool intrinsic);
+ void computeRelationOnOne(const overworld::Triplet& pattern, overworld::GetRelations::Response& response, bool deictic, bool intrinsic);
+ void computeDeicticRelation(Object* object_a, Object* object_b, overworld::GetRelations::Response& response);
+
+ bool shouldBeTested(Object* object);
+ bool isNextTo(Object* object_a, Object* object_b);
+ bool InOverlappingOnZ(Object* object_a, Object* object_b);
+};
+
+} // namespace owds
+
+#endif // OVERWORLD_RELATIONSSENDER_H
\ No newline at end of file
diff --git a/include/overworld/SituationAssessor.h b/include/overworld/SituationAssessor.h
index 7168ca49..c71cd51d 100644
--- a/include/overworld/SituationAssessor.h
+++ b/include/overworld/SituationAssessor.h
@@ -44,7 +44,9 @@ struct HumanAssessor_t
class SituationAssessor
{
public:
- SituationAssessor(const std::string& agent_name, const std::string& config_path, bool is_robot = false);
+ SituationAssessor(const std::string& agent_name, const std::string& config_path,
+ double assessment_frequency, double simulation_frequency,
+ bool simulate = true, bool is_robot = false);
SituationAssessor(const SituationAssessor& other) = delete;
~SituationAssessor();
@@ -62,6 +64,7 @@ class SituationAssessor
bool is_robot_;
std::string config_path_;
+ bool simulate_;
ros::NodeHandle n_;
ros::CallbackQueue callback_queue_;
@@ -70,6 +73,7 @@ class SituationAssessor
ros::ServiceServer bounding_box_service_;
std::atomic run_;
double time_step_; // in second
+ double simu_step_;
BulletClient* bullet_client_;
PerceptionManager perception_manager_;
@@ -86,6 +90,7 @@ class SituationAssessor
void assessmentLoop();
void assess();
+ void processHumans(std::map>& agents_segmentation_ids);
void updateHumansPerspective(const std::string& human_name,
const std::map& objects,
const std::map& humans,
diff --git a/include/overworld/Utility/BulletKeypressHandler.h b/include/overworld/Utility/BulletKeypressHandler.h
index c63025aa..555a5051 100644
--- a/include/overworld/Utility/BulletKeypressHandler.h
+++ b/include/overworld/Utility/BulletKeypressHandler.h
@@ -8,7 +8,7 @@
namespace owds {
-inline void onSpacebarPressed(BulletClient* bullet_client, RobotsPerceptionManager& robots_manager)
+inline void onSpacebarPressed(BulletClient* bullet_client, const RobotsPerceptionManager& robots_manager)
{
auto agents = robots_manager.getAgents();
if(agents.size() != 1)
@@ -27,7 +27,7 @@ inline void onSpacebarPressed(BulletClient* bullet_client, RobotsPerceptionManag
}
}
-inline void handleKeypress(BulletClient* bullet_client, RobotsPerceptionManager& robots_manager)
+inline void handleKeypress(BulletClient* bullet_client, const RobotsPerceptionManager& robots_manager)
{
b3KeyboardEventsData keyboard = bullet_client->getKeyboardEvents();
for (size_t i = 0; i < keyboard.m_numKeyboardEvents; i++)
diff --git a/include/overworld/Utility/Parameters.h b/include/overworld/Utility/Parameters.h
new file mode 100644
index 00000000..3aca1c3c
--- /dev/null
+++ b/include/overworld/Utility/Parameters.h
@@ -0,0 +1,182 @@
+#ifndef OVERWORLD_PARAMETERS_H
+#define OVERWORLD_PARAMETERS_H
+
+#include
+#include
+#include
+#include
+
+#include "overworld/Utility/ShellDisplay.h"
+
+namespace owds
+{
+
+class Parameter
+{
+public:
+ std::string name_;
+ std::vector options_;
+ std::vector values_;
+ std::vector default_values_;
+
+ Parameter(const std::string& name, const std::vector& options, const std::vector& default_values = {}) : name_(name),
+ options_(options),
+ default_values_(default_values)
+ {}
+
+ Parameter(const Parameter& other) : name_(other.name_),
+ options_(other.options_),
+ values_(other.values_),
+ default_values_(other.default_values_)
+ {}
+
+ void insert(const std::string& value) { values_.push_back(value); }
+
+ std::string getFirst()
+ {
+ if(values_.size() == 0)
+ return (default_values_.size() ? default_values_[0] : "");
+ else
+ return (values_.size() ? values_[0] : "");
+ }
+
+ std::vector get()
+ {
+ if(values_.size() == 0)
+ return default_values_;
+ else
+ return values_;
+ }
+
+ bool testOption(const std::string& option)
+ {
+ return std::any_of(options_.begin(), options_.end(), [option](auto op){ return option == op; });
+ }
+
+ bool isValid()
+ {
+ return ((default_values_.size() != 0) || (values_.size() != 0));
+ }
+
+ void display()
+ {
+ ShellDisplay::info(name_ + ":");
+
+ if(values_.size())
+ {
+ for(auto value : values_)
+ ShellDisplay::info("\t- " + value);
+ }
+ else
+ {
+ for(auto value : default_values_)
+ ShellDisplay::info("\t- " + value);
+ }
+ }
+};
+
+class Parameters
+{
+private:
+ std::map parameters_;
+ std::string default_param_name_;
+ std::string process_name_;
+
+public:
+
+ /// @brief Register a new parameter model in the parameter set
+ /// @param param is the parameter object to insert
+ void insert(const Parameter& param)
+ {
+ parameters_.insert(std::pair(param.name_,param));
+ if(param.options_.size() == 0)
+ default_param_name_ = param.name_;
+ }
+
+ /// @brief Returns the parameter object related to the name provided in argument
+ /// @param parameter is the name of the parameter to get
+ /// @return A copy of the parameter object
+ Parameter at(const std::string& parameter)
+ {
+ return parameters_.at(parameter);
+ }
+
+ /// @brief Sets/Reads the values of the parameters
+ /// @param argc the number of strings pointed to by argv
+ /// @param argv is the array of arguments
+ bool set(int argc, char** argv)
+ {
+ process_name_ = std::string(argv[0]);
+ size_t pose;
+ while ((pose = process_name_.find("/")) != std::string::npos) {
+ process_name_ = process_name_.substr(pose+1);
+ }
+ process_name_ = " " + process_name_ + " ";
+
+ for(size_t i = 1; i < (size_t)argc; i++)
+ {
+ if(argv[i][0] == '-')
+ {
+ std::string param_name = "";
+ for(auto param : parameters_)
+ if(param.second.testOption(std::string(argv[i])))
+ {
+ param_name = param.second.name_;
+ break;
+ }
+
+ if(param_name == "")
+ ShellDisplay::warning("unknow option " + std::string(argv[i]));
+ else
+ {
+ if(i+1 < (size_t)argc)
+ {
+ i++;
+ parameters_.at(param_name).insert(std::string(argv[i]));
+ }
+ }
+ }
+ else
+ {
+ if(default_param_name_ != "")
+ parameters_.at(default_param_name_).insert(std::string(argv[i]));
+ else
+ ShellDisplay::warning("No default parameter");
+ }
+ }
+
+ bool is_valid = true;
+ for(auto& param : parameters_)
+ if(param.second.isValid() == false)
+ {
+ std::string option_list;
+ for(auto& option : param.second.options_)
+ {
+ if(option_list != "")
+ option_list += ", ";
+ option_list += option;
+ }
+ ShellDisplay::error(param.second.name_ + " parameter has to be setted. Use one of the following to set it: " + option_list);
+ is_valid = false;
+ }
+
+ return is_valid;
+ }
+
+ /// @brief Displays the parameters names and setted values
+ void display()
+ {
+ std::string delim = "****************";
+ std::string delim_gap;
+ for(size_t i = 0; i < process_name_.size(); i++)
+ delim_gap += "*";
+ ShellDisplay::info(delim + process_name_ + delim);
+ for(auto param : parameters_)
+ param.second.display();
+ ShellDisplay::info(delim + delim_gap + delim);
+ }
+};
+
+} // namespace owds
+
+#endif // OVERWORLD_PARAMETERS_H
diff --git a/include/overworld/Utility/YamlReader.h b/include/overworld/Utility/YamlReader.h
new file mode 100644
index 00000000..b1e1c7e8
--- /dev/null
+++ b/include/overworld/Utility/YamlReader.h
@@ -0,0 +1,102 @@
+#ifndef OWDS_YAMLREADER_H
+#define OWDS_YAMLREADER_H
+
+#include
+#include
+#include
+#include
+#include
+
+namespace owds
+{
+
+class YamlReader;
+
+class YamlElement
+{
+ friend YamlReader;
+public:
+ YamlElement operator[](const std::string& name)
+ {
+ if(subelem)
+ {
+ if(subelem.value().find(name) != subelem.value().end())
+ return subelem.value()[name];
+ else
+ return YamlElement();
+ }
+ else
+ return YamlElement();
+ }
+
+ std::vector value()
+ {
+ if(data)
+ return data.value();
+ else
+ return {};
+ }
+
+ std::vector getElementsKeys()
+ {
+ std::vector res;
+ if(subelem)
+ std::transform(subelem.value().cbegin(), subelem.value().cend(),
+ std::back_inserter(res),
+ [](const std::pair& elem){ return elem.first;});
+ return res;
+ }
+
+ bool keyExists(const std::string& key)
+ {
+ if(subelem)
+ return (subelem.value().find(key) != subelem.value().end());
+ else
+ return false;
+ }
+
+private:
+ std::experimental::optional> data;
+ std::experimental::optional> subelem;
+};
+
+class YamlReader
+{
+public:
+ bool read(const std::string& path);
+
+ void display();
+
+ YamlElement operator[](const std::string& name)
+ {
+ if(elements_.find(name) != elements_.end())
+ return elements_[name];
+ else
+ return YamlElement();
+ }
+
+ std::vector getKeys()
+ {
+ std::vector res;
+ std::transform(elements_.cbegin(), elements_.cend(),
+ std::back_inserter(res),
+ [](const std::pair& elem){ return elem.first;});
+ return res;
+ }
+
+private:
+ std::map elements_;
+
+ std::map read(const std::vector& lines, size_t& current_line);
+ YamlElement readList(const std::vector& lines, size_t& current_line);
+
+ void display(const std::map& config, size_t nb = 0);
+ void displayElement(const std::pair& it, size_t nb);
+ void displayTab(size_t nb);
+ void removeComment(std::string& line);
+ size_t countSpaces(const std::string& line);
+};
+
+} // namespace owds
+
+#endif // OWDS_YAMLREADER_H
diff --git a/launch/overworld.launch b/launch/overworld.launch
index 074f43c8..a7596964 100644
--- a/launch/overworld.launch
+++ b/launch/overworld.launch
@@ -1,5 +1,7 @@
diff --git a/modules_plugins.xml b/modules_plugins.xml
index bdd523b5..78ba7c77 100644
--- a/modules_plugins.xml
+++ b/modules_plugins.xml
@@ -1,37 +1,43 @@
-
+
- A robot perception module for a PR2 robot.
+ A robot perception module using robot joint state.
Possible parameters are: name, min_period,
right_hand, left_hand, and head.
-
-
+
- A human perception module using the Optitrack mocap system.
- Possible parameters are: name, offset_x,
- offset_y, offset_z.
+ A robot perception module providing fake body parts from a ROS topic.
+ Possible parameters are: name, head, base
+ head_pose, and base_pose.
-
+
- An object perception module using the Ar_track_alvar system.
- Does not take any parameter.
+ A human perception module using a poseStamped ROS message.
+ Possible parameters are: name.
+ The module subscribe to "/peopleTrack". Remap your topic if needed.
-
+
- An object perception module using the Pr2 pressure sensors.
- Possible parameters are: side (0 left / 1 right),
- pressure_threshold, distance_threshold, min_period.
+ A human perception module providing fake body parts from a ROS topic.
+ Does not take any parameter.
+
An object perception module providing static objects.
+ This module requires a 'file' parameter being a yaml description of the assets to be loaded.
+
+
+
+
+ An object perception module providing fake objects from a ROS topic.
Does not take any parameter.
diff --git a/msg/AgentPose.msg b/msg/AgentPose.msg
new file mode 100644
index 00000000..d850b498
--- /dev/null
+++ b/msg/AgentPose.msg
@@ -0,0 +1,2 @@
+string id
+EntityPose[] parts
\ No newline at end of file
diff --git a/msg/EntitiesPoses.msg b/msg/EntitiesPoses.msg
new file mode 100644
index 00000000..5456543c
--- /dev/null
+++ b/msg/EntitiesPoses.msg
@@ -0,0 +1 @@
+EntityPose[] entities
\ No newline at end of file
diff --git a/msg/EntityPose.msg b/msg/EntityPose.msg
new file mode 100644
index 00000000..87c1db0b
--- /dev/null
+++ b/msg/EntityPose.msg
@@ -0,0 +1,2 @@
+string id
+geometry_msgs/PoseStamped pose
\ No newline at end of file
diff --git a/ontologies/common_ground_example.owl b/ontologies/common_ground_example.owl
index 2f6ce329..443daea5 100644
--- a/ontologies/common_ground_example.owl
+++ b/ontologies/common_ground_example.owl
@@ -87,9 +87,9 @@
-
+
-
+
-
-
-
-
overworld
- 0.1.2
+ 0.1.3
The overworld package
Guillaume Sarthou
@@ -23,11 +23,9 @@
roslib
eigen
message_filters
- ar_track_alvar_msgs
sensor_msgs
visualization_msgs
geometry_msgs
- optitrack_msgs
cv_bridge
pluginlib
@@ -42,11 +40,10 @@
bullet
eigen
message_filters
- ar_track_alvar_msgs
sensor_msgs
visualization_msgs
geometry_msgs
- optitrack_msgs
+ optitrack_mss
cv_bridge
@@ -61,11 +58,9 @@
roslib
eigen
message_filters
- ar_track_alvar_msgs
sensor_msgs
visualization_msgs
geometry_msgs
- optitrack_msgs
cv_bridge
pluginlib
diff --git a/src/BasicTypes/Entity.cpp b/src/BasicTypes/Entity.cpp
index 6bd72126..5457c773 100644
--- a/src/BasicTypes/Entity.cpp
+++ b/src/BasicTypes/Entity.cpp
@@ -2,6 +2,8 @@
#include
+#include
+
namespace owds {
Entity::Entity(const std::string& id, bool is_true_id): id_(id),
@@ -56,6 +58,28 @@ const Pose& Entity::pose(unsigned int id) const
return last_poses_.at(last_poses_.size() - id - 1).pose;
}
+const Pose& Entity::pose(const ros::Time& stamp) const
+{
+ if (!is_located_)
+ throw UnlocatedEntityError(id_);
+
+ ros::Duration min_err = stamp - last_poses_.back().stamp;
+
+ for(size_t i = last_poses_.size() - 2; i >= 0; i--)
+ {
+ auto delta = stamp - last_poses_.at(i).stamp;
+ if(delta < min_err)
+ {
+ min_err = delta;
+ std::cout << "take pose at -" << i << std::endl;
+ }
+ else
+ return last_poses_.at(i + 1).pose;
+ }
+
+ return last_poses_.back().pose;
+}
+
bool Entity::hasMoved() const
{
if (!is_located_)
@@ -159,49 +183,51 @@ geometry_msgs::TransformStamped Entity::toTfTransform() const
return transform;
}
-visualization_msgs::Marker Entity::toMarker(int id, double lifetime, const std::string& ns) const
+const visualization_msgs::Marker& Entity::toMarker(int id, double lifetime, const std::string& ns)
{
if (!isLocated())
{
throw std::runtime_error("Called toMarker on a non located entity: '" + id_ + "'.");
}
- visualization_msgs::Marker marker;
- marker.header.frame_id = "map";
- marker.header.stamp = last_poses_.back().stamp;
- marker.id = id;
- marker.lifetime = ros::Duration(lifetime);
+ marker_.lifetime = ros::Duration(lifetime);
+ marker_.header.stamp = last_poses_.back().stamp;
+ marker_.pose = last_poses_.back().pose.toPoseMsg();
+ marker_.ns = ns;
+ return marker_;
+}
+
+void Entity::updateMarker()
+{
+ marker_.header.frame_id = "map";
+ marker_.id = std::hash{}(id_);
switch (shape_.type)
{
case ShapeType_e::SHAPE_MESH:
- marker.type = marker.MESH_RESOURCE;
- marker.mesh_resource = shape_.visual_mesh_resource;
- marker.mesh_use_embedded_materials = true;
+ marker_.type = marker_.MESH_RESOURCE;
+ marker_.mesh_resource = shape_.visual_mesh_resource;
+ marker_.mesh_use_embedded_materials = true;
break;
case ShapeType_e::SHAPE_SPEHERE:
- marker.type = marker.SPHERE;
+ marker_.type = marker_.SPHERE;
break;
case ShapeType_e::SHAPE_CUBE:
- marker.type = marker.CUBE;
+ marker_.type = marker_.CUBE;
break;
case ShapeType_e::SHAPE_CYLINDER:
- marker.type = marker.CYLINDER;
+ marker_.type = marker_.CYLINDER;
break;
default:
- throw std::runtime_error("toMarker has been called on entity '" + id_ + "' + but its ShapeType is not defined.");
break;
}
- marker.scale.x = shape_.scale[0];
- marker.scale.y = shape_.scale[1];
- marker.scale.z = shape_.scale[2];
- marker.color.r = shape_.color[0];
- marker.color.g = shape_.color[1];
- marker.color.b = shape_.color[2];
- marker.color.a = 1.0;
- marker.ns = ns;
- marker.action = marker.ADD;
- marker.pose = last_poses_.back().pose.toPoseMsg();
- return marker;
+ marker_.scale.x = shape_.scale[0];
+ marker_.scale.y = shape_.scale[1];
+ marker_.scale.z = shape_.scale[2];
+ marker_.color.r = shape_.color[0];
+ marker_.color.g = shape_.color[1];
+ marker_.color.b = shape_.color[2];
+ marker_.color.a = 1.0;
+ marker_.action = marker_.ADD;
}
void Entity::computeFeature()
diff --git a/src/BasicTypes/Object.cpp b/src/BasicTypes/Object.cpp
index b742de99..10ca9f57 100644
--- a/src/BasicTypes/Object.cpp
+++ b/src/BasicTypes/Object.cpp
@@ -3,6 +3,8 @@
#include "overworld/BasicTypes/Hand.h"
#include "overworld/Utility/ShellDisplay.h"
+#include
+
namespace owds {
Object::Object(const std::string& id, bool is_true_id): Entity(id, is_true_id),
@@ -72,7 +74,6 @@ void Object::setDefaultMass(double density)
case ShapeType_e::SHAPE_MESH:
mass_ = getBbVolume() * density;
break;
-
case ShapeType_e::SHAPE_SPEHERE:
mass_ = 4.0*M_PI* std::pow(shape_.scale[0], 3) / 3.0;
break;
@@ -88,4 +89,53 @@ void Object::setDefaultMass(double density)
}
}
+void Object::computeCorners()
+{
+ double min_x = origin_offset_[0] - bounding_box_[0] / 2.;
+ double max_x = origin_offset_[0] + bounding_box_[0] / 2.;
+ double min_y = origin_offset_[1] - bounding_box_[1] / 2.;
+ double max_y = origin_offset_[1] + bounding_box_[1] / 2.;
+ double min_z = origin_offset_[2] - bounding_box_[2] / 2.;
+ double max_z = origin_offset_[2] + bounding_box_[2] / 2.;
+ corners_ = { {{min_x, min_y, min_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{max_x, min_y, min_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{min_x, max_y, min_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{max_x, max_y, min_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{min_x, min_y, max_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{max_x, min_y, max_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{min_x, max_y, max_z}, {0.0, 0.0, 0.0, 1.0}},
+ {{max_x, max_y, max_z}, {0.0, 0.0, 0.0, 1.0}} };
+}
+
+double Object::getMinDistanceTo(const Object& other)
+{
+ std::vector other_corners = other.getCorners();
+ double min_dist = std::numeric_limits::max();
+
+ for(auto& c_a : other_corners)
+ {
+ Pose map_to_corner = other.pose() * c_a;
+ Pose in_b = map_to_corner.transformIn(pose());
+ for(auto& c_b : corners_)
+ {
+ double dist = in_b.distanceSqTo(c_b);
+ if(dist < min_dist)
+ min_dist = dist;
+ }
+ }
+
+ return sqrt(min_dist);
+}
+
+void Object::setTypes(const std::vector& types)
+{
+ for(auto& type : types)
+ types_.insert(type);
+}
+
+bool Object::isA(const std::string& type)
+{
+ return (types_.find(type) != types_.end());
+}
+
} // namespace owds
\ No newline at end of file
diff --git a/src/Bullet/BulletClient.cpp b/src/Bullet/BulletClient.cpp
index 250ea89d..6ab35527 100644
--- a/src/Bullet/BulletClient.cpp
+++ b/src/Bullet/BulletClient.cpp
@@ -24,6 +24,7 @@ void BulletClient::setAdditionalSearchPath(const std::string& path)
{
b3SharedMemoryCommandHandle command_handle = b3SetAdditionalSearchPath(*client_handle_, path.c_str());
b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(*client_handle_, command_handle);
+ (void)status_handle;
if(additional_path_ != "")
ShellDisplay::warning("[Bullet] The previous additional path has been overwritten");
@@ -53,7 +54,12 @@ int BulletClient::createVisualShapeCapsule(float radius, float height, const std
int BulletClient::createVisualShapeMesh(const std::string& file_name, const std::array& scale, const std::array& rgba_color)
{
- return createVisualShape(GEOM_MESH, 0, {0,0,0}, 0, getFullPath(file_name), scale, rgba_color);
+ std::string full_path = getFullPath(file_name);
+ std::string hash_str = full_path + std::to_string(scale[0]) + std::to_string(scale[1]) + std::to_string(scale[2]) +
+ std::to_string(rgba_color[0]) + std::to_string(rgba_color[1]) + std::to_string(rgba_color[2]) + std::to_string(rgba_color[3]);
+ size_t hash = std::hash{}(hash_str);
+
+ return createVisualShape(GEOM_MESH, 0, {0,0,0}, 0, full_path, scale, rgba_color);
}
int BulletClient::createVisualShape(BulletShapeType_e shape_type,
@@ -144,7 +150,17 @@ int BulletClient::createCollisionShapeCapsule(float radius, float height, int fl
int BulletClient::createCollisionShapeMesh(const std::string& file_name, const std::array& scale, int flags)
{
- return createCollisionShape(GEOM_MESH, 0, {0}, 0, getFullPath(file_name), scale, flags);
+ std::string full_path = getFullPath(file_name);
+ std::string hash_str = full_path + std::to_string(scale[0]) + std::to_string(scale[1]) +
+ std::to_string(scale[2]) + std::to_string(flags);
+ size_t hash = std::hash{}(hash_str);
+ auto it = loaded_collision_meshes_.find(hash);
+ if(it != loaded_collision_meshes_.end())
+ return it->second;
+
+ int id = createCollisionShape(GEOM_MESH, 0, {0}, 0, full_path, scale, flags);
+ loaded_collision_meshes_.emplace(hash, id);
+ return id;
}
int BulletClient::createCollisionShape(BulletShapeType_e shape_type,
@@ -161,7 +177,7 @@ int BulletClient::createCollisionShape(BulletShapeType_e shape_type,
int shape_index = -1;
b3SharedMemoryCommandHandle command_handle = b3CreateCollisionShapeCommandInit(*client_handle_);
- if (shape_type == GEOM_SPHERE && radius > 0)
+ if (shape_type == GEOM_SPHERE)
{
if(radius > 0)
shape_index = b3CreateCollisionShapeAddSphere(command_handle, radius);
@@ -248,14 +264,24 @@ int BulletClient::createMultiBody(float base_mass,
int BulletClient::loadTexture(const std::string& file_path)
{
+ auto full_path = getFullPath(file_path);
+ size_t hash = std::hash{}(full_path);
+ auto it = loaded_textures_.find(hash);
+ if(it != loaded_textures_.end())
+ return it->second;
+
std::lock_guard lock(mutex_);
- b3SharedMemoryCommandHandle command_handle = b3InitLoadTexture(*client_handle_, getFullPath(file_path).c_str());
+ b3SharedMemoryCommandHandle command_handle = b3InitLoadTexture(*client_handle_, full_path.c_str());
b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(*client_handle_, command_handle);
int status_type = b3GetStatusType(status_handle);
if (status_type == CMD_LOAD_TEXTURE_COMPLETED)
- return b3GetStatusTextureUniqueId(status_handle);
+ {
+ int id = b3GetStatusTextureUniqueId(status_handle);
+ loaded_textures_.emplace(hash, id);
+ return id;
+ }
- ShellDisplay::error("[Bullet] Error loading texture \'" + getFullPath(file_path) + "\'");
+ ShellDisplay::error("[Bullet] Error loading texture \'" + full_path + "\'");
return -1;
}
@@ -366,7 +392,7 @@ int BulletClient::loadURDFRaw(const std::string& raw_urdf, const std::string& te
// Return the number of joints in an object based on
// body index; body index is based on order of sequence
-// the object is loaded into simulation
+// the object is loaded into screateMultiBodyimulation
int BulletClient::getNumJoints(int body_id)
{
std::lock_guard lock(mutex_);
@@ -695,10 +721,7 @@ struct b3CameraImageData BulletClient::getCameraImage(int width, int height,
std::unordered_set BulletClient::getSegmentationIds(const b3CameraImageData& image)
{
- std::unordered_set segmentation_ids;
- for(size_t i = 0; i < image.m_pixelHeight*image.m_pixelWidth; i++)
- segmentation_ids.insert(image.m_segmentationMaskValues[i]);
-
+ std::unordered_set segmentation_ids(image.m_segmentationMaskValues, image.m_segmentationMaskValues+image.m_pixelHeight*image.m_pixelWidth);
return segmentation_ids;
}
diff --git a/src/Facts/FactsCalculator.cpp b/src/Facts/FactsCalculator.cpp
index 1f63eb39..6ffb65af 100644
--- a/src/Facts/FactsCalculator.cpp
+++ b/src/Facts/FactsCalculator.cpp
@@ -2,26 +2,20 @@
namespace owds {
-FactsCalculator::FactsCalculator(ros::NodeHandle* nh, const std::string& agent_name): ontos_(OntologiesManipulator(nh))
-{
- ontos_.waitInit();
- ontos_.add(agent_name);
- onto_ = ontos_.get(agent_name);
- onto_->close();
-
-}
+FactsCalculator::FactsCalculator(const std::string& agent_name)
+{}
-std::vector FactsCalculator::computeFacts(const std::map& objects,
- const std::map& agents,
- const std::map>& segmantation_ids)
+std::vector FactsCalculator::computeObjectsFacts(const std::map& objects,
+ bool clear)
{
- facts_.clear();
+ if(clear)
+ facts_.clear();
for(auto& obj_from : objects)
{
if(obj_from.second->isInHand())
continue;
- else if(obj_from.second->isStatic())
+ else if(isValid(obj_from.second) == false)
continue;
bool is_in = false;
@@ -30,7 +24,7 @@ std::vector FactsCalculator::computeFacts(const std::mapisInHand())
continue;
- else if(obj_to.second->isStatic())
+ else if(isValid(obj_to.second) == false)
continue;
if(obj_from.first != obj_to.first)
@@ -42,7 +36,7 @@ std::vector FactsCalculator::computeFacts(const std::mapisInHand())
continue;
- else if(obj_to.second->isStatic())
+ else if(isValid(obj_to.second) == false)
continue;
if(obj_from.first != obj_to.first)
@@ -50,12 +44,23 @@ std::vector FactsCalculator::computeFacts(const std::map FactsCalculator::computeAgentsFacts(const std::map& objects,
+ const std::map& agents,
+ const std::map>& segmantation_ids,
+ bool clear)
+{
+ if(clear)
+ facts_.clear();
+
for(auto& agent_from : agents)
{
isInHand(agent_from.second);
for (auto& obj: objects)
{
- if(obj.second->isStatic())
+ if(isValid(obj.second) == false)
continue;
if (agent_from.second->getType() == AgentType_e::HUMAN){
@@ -80,11 +85,13 @@ std::vector FactsCalculator::computeFacts(const std::mapisAabbValid() == false) || (object_on->isAabbValid() == false))
+ if(object_under->isA("Support") == false)
+ return false;
+ else if((object_under->isAabbValid() == false) || (object_on->isAabbValid() == false))
return false;
else if(overlapXY(object_under->getAabb(), object_on->getAabb()) == false)
return false;
- else if(std::abs(object_under->getAabb().max[2] - object_on->getAabb().min[2]) > 0.04)
+ else if(std::abs(object_under->getAabb().max[2] - object_on->getAabb().min[2]) > 0.08)
return false;
else
{
@@ -95,7 +102,9 @@ bool FactsCalculator::isOnTopfOf(Object* object_under, Object* object_on)
bool FactsCalculator::isInContainer(Object* object_around, Object* object_in)
{
- if((object_around->isAabbValid() == false) || (object_in->isAabbValid() == false))
+ if(object_around->isA("Container") == false)
+ return false;
+ else if((object_around->isAabbValid() == false) || (object_in->isAabbValid() == false))
return false;
else if(object_in->getAabb().min[2] < object_around->getAabb().min[2] - 0.06)
return false;
@@ -216,8 +225,7 @@ bool FactsCalculator::hasInHand(Agent* agent, Object* object)
{
if (leftHand->pose().distanceTo(object->pose()) < 0.08)
{
- std::vector types = onto_->individuals.getUp(object->id());
- if (std::find(types.begin(), types.end(), "Pickable") != types.end())
+ if(object->isA("Pickable"))
{
object->setInHand(leftHand);
leftHand->putInHand(object);
@@ -240,8 +248,7 @@ bool FactsCalculator::hasInHand(Agent* agent, Object* object)
{
if (rightHand->pose().distanceTo(object->pose()) < 0.08)
{
- std::vector types = onto_->individuals.getUp(object->id());
- if (std::find(types.begin(), types.end(), "Pickable") != types.end())
+ if(object->isA("Pickable"))
{
object->setInHand(rightHand);
rightHand->putInHand(object);
@@ -318,4 +325,12 @@ bool FactsCalculator::isHandMovingTowards(Agent* agent, Object* object)
return ret;
}
+bool FactsCalculator::isValid(Object* object)
+{
+ if(object->isStatic())
+ return false;
+ else
+ return object->isLocated();
+}
+
} // namespace owds
\ No newline at end of file
diff --git a/src/Facts/Publisher/FactsPublisher.cpp b/src/Facts/Publisher/FactsPublisher.cpp
index db663e74..dd6ae084 100644
--- a/src/Facts/Publisher/FactsPublisher.cpp
+++ b/src/Facts/Publisher/FactsPublisher.cpp
@@ -1,15 +1,20 @@
#include "overworld/Facts/Publisher/FactsPublisher.h"
+#include
+#include
+
namespace owds {
void FactsPublisher::publish(const std::vector& facts)
{
- std::unordered_set done_facts_;
+ std::unordered_set done_facts;
std::vector to_insert;
- for(auto& fact : facts)
+ auto filtered_facts = filterIncomingFacts(facts);
+
+ for(auto& fact : filtered_facts)
{
- if(fact.getPredicate() == "pick")
+ /*if(fact.getPredicate() == "pick")
{
if(held_by_.find(fact.getObject()) == held_by_.end())
{
@@ -17,7 +22,7 @@ void FactsPublisher::publish(const std::vector& facts)
to_insert.push_back(fact);
}
}
- else if((fact.getPredicate() == "place") && (fact.getPredicate() == "release"))
+ else if((fact.getPredicate() == "place") || (fact.getPredicate() == "release"))
{
if(held_by_.find(fact.getObject()) != held_by_.end())
{
@@ -25,33 +30,93 @@ void FactsPublisher::publish(const std::vector& facts)
removeFromKb(fact);
}
}
- else
+ else*/
{
if(pending_facts_.find(fact.getHash()) == pending_facts_.end())
{
- pending_facts_.insert(std::make_pair(fact.getHash(), fact));
+ pending_facts_.emplace(fact.getHash(), fact);
to_insert.push_back(fact);
}
- done_facts_.insert(fact.getHash());
+ done_facts.insert(fact.getHash());
}
}
std::unordered_set to_remove;
for(auto fact_it : pending_facts_)
{
- if(done_facts_.find(fact_it.first) == done_facts_.end())
+ if(done_facts.find(fact_it.first) == done_facts.end())
to_remove.insert(fact_it.first);
}
- for(auto hash : to_remove)
+ auto stable_to_remove = filterOutgoingFacts(to_remove);
+ for(auto hash : stable_to_remove)
{
removeFromKb(pending_facts_.at(hash));
pending_facts_.erase(hash);
}
- for(auto fact : to_insert)
+ for(const auto& fact : to_insert)
addToKb(fact);
}
+std::vector FactsPublisher::filterIncomingFacts(const std::vector& facts)
+{
+ auto buf_fact_it = facts_buffer_.begin();
+ for(; buf_fact_it != facts_buffer_.end();)
+ {
+ auto found_it = std::find_if(facts.begin(), facts.end(), [buf_fact_it](const auto& fact) { return fact == buf_fact_it->first; });
+
+ if(found_it == facts.end())
+ buf_fact_it = facts_buffer_.erase(buf_fact_it);
+ else
+ ++buf_fact_it;
+ }
+
+ std::vector stable_facts;
+ for(auto& fact : facts)
+ {
+ auto buf_it = facts_buffer_.find(fact);
+ if(buf_it == facts_buffer_.end())
+ facts_buffer_.emplace(fact, 0);
+ else if(buf_it->second == 3)
+ stable_facts.emplace_back(fact);
+ else
+ buf_it->second++;
+ }
+
+ return stable_facts;
+}
+
+std::unordered_set FactsPublisher::filterOutgoingFacts(const std::unordered_set& facts)
+{
+ auto buf_fact_it = rmv_buffer_.begin();
+ for(; buf_fact_it != rmv_buffer_.end();)
+ {
+ auto found_it = std::find_if(facts.begin(), facts.end(), [buf_fact_it](const auto& fact) { return fact == buf_fact_it->first; });
+
+ if(found_it == facts.end())
+ buf_fact_it = rmv_buffer_.erase(buf_fact_it);
+ else
+ ++buf_fact_it;
+ }
+
+ std::unordered_set stable_facts;
+ for(auto& fact : facts)
+ {
+ auto buf_it = rmv_buffer_.find(fact);
+ if(buf_it == rmv_buffer_.end())
+ rmv_buffer_.emplace(fact, 0);
+ else if(buf_it->second == 3)
+ {
+ stable_facts.insert(fact);
+ rmv_buffer_.erase(buf_it);
+ }
+ else
+ buf_it->second++;
+ }
+
+ return stable_facts;
+}
+
} // namespace owds
\ No newline at end of file
diff --git a/src/Nodes/overworld.cpp b/src/Nodes/overworld.cpp
index 0c02334a..f9388cff 100644
--- a/src/Nodes/overworld.cpp
+++ b/src/Nodes/overworld.cpp
@@ -1,6 +1,7 @@
#include
#include "overworld/Utility/ShellDisplay.h"
+#include "overworld/Utility/Parameters.h"
#include
#include
@@ -26,16 +27,27 @@ int main(int argc, char** argv)
signal(SIGABRT, handler);
ros::init(argc, argv, "overworld");
- if(argc < 3)
+ owds::Parameters params;
+ params.insert(owds::Parameter("config_path", {"-c", "--config"}));
+ params.insert(owds::Parameter("robot_name", {"-n", "--name"}));
+ params.insert(owds::Parameter("simulate", {"-s", "--simulate"}, {"true"}));
+ params.insert(owds::Parameter("assessment frequency", {"-af", "--assessment-frequency"}, {"17"}));
+ params.insert(owds::Parameter("simulation frequency", {"-sf", "--simulation-frequency"}, {"70"}));
+
+ bool valid_parameters = params.set(argc, argv);
+ params.display();
+ if(valid_parameters == false)
{
- owds::ShellDisplay::error("A configuration file and the robot name should be provided");
+ owds::ShellDisplay::error("Some parameters have not been setted. Overworld will shutdown.");
return -1;
}
- std::string config_path = std::string(argv[1]);
- std::string robot_name = std::string(argv[2]);
-
- owds::SituationAssessor robot_situation_assessor(robot_name, config_path, true);
+ owds::SituationAssessor robot_situation_assessor(params.at("robot_name").getFirst(),
+ params.at("config_path").getFirst(),
+ std::stod(params.at("assessment frequency").getFirst()),
+ std::stod(params.at("simulation frequency").getFirst()),
+ params.at("simulate").getFirst() == "true",
+ true);
robot_situation_assessor.run();
diff --git a/src/Nodes/plugins.cpp b/src/Nodes/plugins.cpp
new file mode 100644
index 00000000..7f4cd5cb
--- /dev/null
+++ b/src/Nodes/plugins.cpp
@@ -0,0 +1,201 @@
+#include "overworld/Utility/ShellDisplay.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
+#include "overworld/BasicTypes/BodyPart.h"
+#include "overworld/BasicTypes/Object.h"
+
+#include
+
+#include
+
+void displayUsage()
+{
+ std::cout << std::endl << "usage: rosrun overworld plugins COMMAND [ARGS]" << std::endl << std::endl;
+ std::cout << "The overworld plugins commands are:" << std::endl;
+ std::cout << "\tlist List available plugins in your current workspace" << std::endl;
+ std::cout << "\tdescription Given a module name, provide its description" << std::endl;
+ std::cout << "\tlibrary_path Given a module name, provide the path to its associated library" << std::endl;
+ std::cout << "\tpackage Given a module name, provide the name of the containing package" << std::endl;
+ std::cout << std::endl;
+ std::cout << "See \'rosrun overworld plugins help COMMAND\' for more information on a specific command." << std::endl;
+ std::cout << std::endl;
+}
+
+void help(const std::string& command)
+{
+ std::cout << std::endl << "NAME" << std::endl;
+ if(command == "list")
+ std::cout << "\tplugins-list" << std::endl;
+ else if(command == "description")
+ std::cout << "\tplugins-description" << std::endl;
+ else if(command == "library_path")
+ std::cout << "\tplugins-library_path" << std::endl;
+ else if(command == "package")
+ std::cout << "\tplugins-package" << std::endl;
+ else
+ {
+ std::cout << "\tUNKNOWN COMMAND" << std::endl;
+ return;
+ }
+
+ std::cout << std::endl << "SYNOPSIS" << std::endl;
+ if(command == "list")
+ std::cout << "\trosrun overworld plugins list []" << std::endl;
+ else if(command == "description")
+ std::cout << "\trosrun overworld plugins description " << std::endl;
+ else if(command == "library_path")
+ std::cout << "\trosrun overworld plugins library_path " << std::endl;
+ else if(command == "package")
+ std::cout << "\trosrun overworld plugins package " << std::endl;
+
+ std::cout << std::endl << "DESCRIPTION" << std::endl;
+ if(command == "list")
+ std::cout << "\tThis command lists the available perception modules in the form of plugins." << std::endl;
+ else if(command == "description")
+ std::cout << "\tGiven a module name, this command provides its description." << std::endl;
+ else if(command == "library_path")
+ std::cout << "\tGiven a module name, this command provides the path to its associated library." << std::endl;
+ else if(command == "package")
+ std::cout << "\tGiven a module name, this command provides the name of the containing package" << std::endl;
+
+ std::cout << std::endl << "OPTIONS" << std::endl;
+ if(command == "list")
+ std::cout << "\t" << std::endl << "\t\tEither Objects or BodyParts to focus on specific module type." << std::endl;
+ else
+ std::cout << "\t" << std::endl << "\t\tThe name of an existing perception module." << std::endl;
+
+ std::cout << std::endl;
+}
+
+void list(bool objects, bool bodyparts)
+{
+ std::cout << std::endl;
+ if(bodyparts)
+ {
+ std::cout << "Available body part perception modules are:" << std::endl;
+ pluginlib::ClassLoader> loader_bodyparts("overworld", "owds::PerceptionModuleBase_");
+ std::vector modules = loader_bodyparts.getDeclaredClasses();
+ for(auto& module : modules)
+ {
+ std::cout << "\t- " << module << std::endl;
+ }
+ std::cout << std::endl;
+ }
+
+ if(objects)
+ {
+ std::cout << "Available body part perception modules are:" << std::endl;
+ pluginlib::ClassLoader> loader_objects("overworld", "owds::PerceptionModuleBase_");
+ std::vector modules = loader_objects.getDeclaredClasses();
+ for(auto& module : modules)
+ {
+ std::cout << "\t- " << module << std::endl;
+ }
+ std::cout << std::endl;
+ }
+}
+
+void description(const std::string& _class)
+{
+ std::string description;
+ pluginlib::ClassLoader> loader_bodyparts("overworld", "owds::PerceptionModuleBase_");
+ description = loader_bodyparts.getClassDescription(_class);
+
+ if(description == "")
+ {
+ pluginlib::ClassLoader> loader_objects("overworld", "owds::PerceptionModuleBase_");
+ description = loader_objects.getClassDescription(_class);
+ }
+
+ if(description == "")
+ std::cout << std::endl << "Module " << _class << " does not exist. See command \'list\' to see available modules." << std::endl << std::endl;
+ else
+ std::cout << std::endl << "Module " << _class << ":" << std::endl << "\t" << description << std::endl << std::endl;
+}
+
+void libraryPath(const std::string& _class)
+{
+ std::string path;
+ pluginlib::ClassLoader> loader_bodyparts("overworld", "owds::PerceptionModuleBase_");
+ path = loader_bodyparts.getClassLibraryPath(_class);
+
+ if(path == "")
+ {
+ pluginlib::ClassLoader> loader_objects("overworld", "owds::PerceptionModuleBase_");
+ path = loader_objects.getClassLibraryPath(_class);
+ }
+
+ if(path == "")
+ std::cout << std::endl << "Module " << _class << " does not exist. See command \'list\' to see available modules." << std::endl << std::endl;
+ else
+ std::cout << std::endl << "Module " << _class << ":" << std::endl << "\t" << path << std::endl << std::endl;
+}
+
+void classPackage(const std::string& _class)
+{
+ std::string package;
+ pluginlib::ClassLoader> loader_bodyparts("overworld", "owds::PerceptionModuleBase_");
+ package = loader_bodyparts.getClassPackage(_class);
+
+ if(package == "")
+ {
+ pluginlib::ClassLoader> loader_objects("overworld", "owds::PerceptionModuleBase_");
+ package = loader_objects.getClassPackage(_class);
+ }
+
+ if(package == "")
+ std::cout << std::endl << "Module " << _class << " does not exist. See command \'list\' to see available modules." << std::endl << std::endl;
+ else
+ std::cout << std::endl << "Module " << _class << ":" << std::endl << "\t" << package << std::endl << std::endl;
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "plugins");
+
+ if(argc == 1)
+ displayUsage();
+ else
+ {
+ std::string command = std::string(argv[1]);
+ if(command == "list")
+ {
+ if(argc == 2)
+ list(true, true);
+ else if(argc > 2)
+ {
+ std::string option = std::string(argv[2]);
+ list(option == "Objects", option == "BodyParts");
+ }
+ }
+ else if(command == "description")
+ {
+ if(argc < 3)
+ std::cout << std::endl << "No enough arguments. See \'rosrun overworld plugins help COMMAND\' for more information on a specific command." << std::endl << std::endl;
+ else
+ description(std::string(argv[2]));
+ }
+ else if(command == "library_path")
+ {
+ if(argc < 3)
+ std::cout << std::endl << "No enough arguments. See \'rosrun overworld plugins help COMMAND\' for more information on a specific command." << std::endl << std::endl;
+ else
+ libraryPath(std::string(argv[2]));
+ }
+ else if(command == "package")
+ {
+ if(argc < 3)
+ std::cout << std::endl << "No enough arguments. See \'rosrun overworld plugins help COMMAND\' for more information on a specific command." << std::endl << std::endl;
+ else
+ classPackage(std::string(argv[2]));
+ }
+ else if(command == "help")
+ {
+ if(argc == 3)
+ help(std::string(argv[2]));
+ else
+ displayUsage();
+ }
+ }
+
+ return 0;
+}
\ No newline at end of file
diff --git a/src/OntologeniusPlugins/ReasonerEgocentric.cpp b/src/OntologeniusPlugins/ReasonerEgocentric.cpp
index cb6e97f0..91899841 100644
--- a/src/OntologeniusPlugins/ReasonerEgocentric.cpp
+++ b/src/OntologeniusPlugins/ReasonerEgocentric.cpp
@@ -9,7 +9,8 @@ namespace ontologenius {
void ReasonerEgocentric::initialize()
{
- std::vector computable_properties = {"egocentricGeometricalRelation"};
+ overworld_client_ = n_.serviceClient 
+ 
+