0.1.1

sarthou · Feb 15, 2022 · 10e0d9d · 10e0d9d
2 parents af02ca7 + b1066b9
commit 10e0d9d
Show file tree

Hide file tree

Showing 58 changed files with 1,704 additions and 452 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 3.0.2)
 set(CMAKE_EXPORT_COMPILE_COMMANDS 1)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 project(overworld)
 
 add_compile_options(-std=c++14)
@@ -23,11 +24,14 @@ find_package(catkin REQUIRED COMPONENTS
   ar_track_alvar_msgs
   sensor_msgs
   visualization_msgs
-  image_transport
   pr2_motion_tasks_msgs
+  cv_bridge
+  pluginlib
 )
 find_package (Eigen3 REQUIRED NO_MODULE)
 find_package(Threads REQUIRED)
+find_package(CURL REQUIRED)
+find_package(pluginlib REQUIRED)
 
 list(APPEND BULLET_LIB_NAMES BulletRoboticsGUI
                              BulletExampleBrowserLib
@@ -65,11 +69,11 @@ add_message_files(
   StampedStringTest.msg
 )
 
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  StartStopModules.srv
+  BoundingBox.srv
+)
 
 generate_messages(
   DEPENDENCIES
@@ -101,6 +105,7 @@ include_directories(
   ${EIGEN3_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
   ${optitrack_ros_INCLUDE_DIRS}
+  ${CURL_INCLUDE_DIRS}
 )
 add_definitions( ${EIGEN3_DEFINITIONS} )
 
@@ -121,6 +126,7 @@ add_library(${PROJECT_NAME}_bullet_lib STATIC
   src/Bullet/BulletClient.cpp
 )
 target_link_libraries(${PROJECT_NAME}_bullet_lib PUBLIC Bullet3)
+set_target_properties(${PROJECT_NAME}_bullet_lib PROPERTIES POSITION_INDEPENDENT_CODE ON)
 
 add_library(${PROJECT_NAME}_types_lib STATIC
   src/Geometry/Pose.cpp
@@ -132,18 +138,16 @@ add_library(${PROJECT_NAME}_types_lib STATIC
 target_link_libraries(${PROJECT_NAME}_types_lib PUBLIC
                       ${catkin_LIBRARIES}
                       ${PROJECT_NAME}_bullet_lib)
+set_target_properties(${PROJECT_NAME}_types_lib PROPERTIES POSITION_INDEPENDENT_CODE ON)
 
 add_library(${PROJECT_NAME}_perception_lib STATIC
-  src/Perception/RobotsPerceptionManager.cpp
-  src/Perception/ObjectsPerceptionManager.cpp
-  src/Perception/HumansPerceptionManager.cpp
-  src/Perception/Modalities/PR2JointsPerception.cpp
-  src/Perception/Modalities/ArTrackPerceptionModule.cpp
-  src/Perception/Modalities/StaticObjectsPerceptionModule.cpp
-  src/Perception/Modalities/OptitrackPerceptionModule.cpp
-  src/Perception/Modalities/ObjectsEmulatedPerceptionModule.cpp
-  src/Perception/Modalities/HumansEmulatedPerceptionModule.cpp
-  src/Perception/Modalities/Pr2GripperPerceptionModule.cpp
+  src/Perception/Managers/ObjectsPerceptionManager.cpp
+  src/Perception/Managers/RobotsPerceptionManager.cpp
+  src/Perception/Managers/HumansPerceptionManager.cpp
+  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}
@@ -163,10 +167,28 @@ target_link_libraries(${PROJECT_NAME}_facts_lib PUBLIC
 add_library(${PROJECT_NAME}_sender_lib STATIC
   src/Senders/ROSSender.cpp
   src/Senders/MotionPlanningPoseSender.cpp
+  src/Senders/Bernie.cpp
 )
 target_link_libraries(${PROJECT_NAME}_sender_lib PUBLIC
                       ${catkin_LIBRARIES}
                       Bullet3 # For b3CameraImage
+                      ${CURL_LIBRARIES}
+                      ${PROJECT_NAME}_types_lib)
+
+#################
+#    Plugins    #
+#################
+
+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
+)
+target_link_libraries(overworld_modules_plugin PUBLIC
+                      ${catkin_LIBRARIES}
+                      ${PROJECT_NAME}_bullet_lib
                       ${PROJECT_NAME}_types_lib)
 
 #################
@@ -197,9 +219,9 @@ target_link_libraries(${PROJECT_NAME}_multi_server_test PRIVATE
                       ${PROJECT_NAME}_bullet_lib
                       ${catkin_LIBRARIES})
 
-add_executable(${PROJECT_NAME}_perception_test src/TestFiles/perception_test.cpp )
-target_link_libraries(${PROJECT_NAME}_perception_test PRIVATE
+add_executable(${PROJECT_NAME}_config_test src/TestFiles/config_test.cpp )
+target_link_libraries(${PROJECT_NAME}_config_test PRIVATE
                       ${PROJECT_NAME}_types_lib
                       ${PROJECT_NAME}_perception_lib
                       ${catkin_LIBRARIES})
-add_dependencies(${PROJECT_NAME}_perception_test overworld_gencpp)
+add_dependencies(${PROJECT_NAME}_config_test overworld_gencpp)
diff --git a/Readme.md b/Readme.md
@@ -1,12 +1,15 @@
+# <img src="docs/images/overworld.png" width="150">
+
 ## Install bullet
 
 ```
-cmake .. -DCMAKE_INSTALL_PREFIX=your/install/path
+cmake .. -DCMAKE_INSTALL_PREFIX=your/install/path -DBUILD_SHARED_LIBS=ON
 make install
 ```
 
 then
 
 ```
 export BULLET_INSTALL_PATH=your/install/path
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$BULLET_INSTALL_PATH/lib
 ```
diff --git a/config/config_example.yaml b/config/config_example.yaml
@@ -0,0 +1,31 @@
+modules:
+  robot:
+    PR2JointsPerception: pr2_joints
+  humans:
+    OptitrackPerceptionModule: optitrack
+  objects:
+    ArTrackPerceptionModule: ar_track
+    Pr2GripperPerceptionModule:
+      - pr2_left_gripper
+      - pr2_right_gripper
+    StaticObjectsPerceptionModule: static
+
+pr2_joints:
+  name : pr2_robot
+  right_hand : r_gripper_tool_frame
+  left_hand : l_gripper_tool_frame
+  head : head_mount_kinect2_rgb_optical_frame
+
+pr2_left_gripper:
+  side: 0 # = left
+  pressure_threshold: 400
+  distance_threshold: 0.03
+
+pr2_right_gripper:
+  side: 1 # = right
+  distance_threshold: 0.03
+
+optitrack:
+  name: human_0
+  offset_x: 6.4868
+  offset_y: 2.8506
diff --git a/docs/images/overworld.png b/docs/images/overworld.png
diff --git a/docs/images/overworld.svg b/docs/images/overworld.svg
diff --git a/include/overworld/BasicTypes/Agent.h b/include/overworld/BasicTypes/Agent.h
@@ -9,7 +9,7 @@ namespace owds {
 
 enum AgentType_e
 {
-  PR2_ROBOT = 0,
+  ROBOT = 0,
   HUMAN
 };
 class Agent

diff --git a/include/overworld/BasicTypes/Entity.h b/include/overworld/BasicTypes/Entity.h
@@ -37,6 +37,8 @@ class Entity
     ros::Time lastStamp() const { return last_poses_.back().stamp; }
     bool hasMoved() const;
 
+    std::array<double, 3> computeTranslationSpeed() const;
+
     void setId(const std::string& id, bool is_true_id = true);
     std::string id() const { return id_; }
     bool isTrueId() const { return is_true_id_; }

diff --git a/include/overworld/BasicTypes/Object.h b/include/overworld/BasicTypes/Object.h
@@ -30,10 +30,14 @@ class Object: public Entity
 
     void merge(Object* other);
 
+    void setBoundingBox(const std::array<double, 3>& bb) { bounding_box_ = bb; }
+    std::array<double, 3> getBoundingBox() { return bounding_box_; }
+
 protected:
     std::vector<PointOfInterest> points_of_interest_;
     bool is_static_;
     Hand* hand_in_;
+    std::array<double, 3> bounding_box_;
 };
 
 } // namespace owds

diff --git a/include/overworld/Facts/FactsCalculator.h b/include/overworld/Facts/FactsCalculator.h
@@ -15,7 +15,7 @@ namespace owds {
 class FactsCalculator
 {
 public:
-  FactsCalculator(ros::NodeHandle* nh);
+  FactsCalculator(ros::NodeHandle* nh, const std::string& agent_name);
   std::vector<Fact> computeFacts(const std::map<std::string, Object*>& objects,
                                  const std::map<std::string, Agent*>& agents,
                                  const std::map<std::string, std::unordered_set<int>>& segmantation_ids);
@@ -35,6 +35,7 @@ class FactsCalculator
 
   bool isLookingAt(Agent* agent, const std::unordered_set<int>& seen_bullet_ids, const Object* object);
   bool hasInHand(Agent* agent, Object* object);
+  bool isHandMovingTowards(Agent* agent, Object* object);
 };
 
 } // namespace owds

diff --git a/include/overworld/Geometry/Pose.h b/include/overworld/Geometry/Pose.h
@@ -46,6 +46,8 @@ class Pose
      */
     double angularDistance(const Pose& pose) const;
 
+    std::array<double, 3> subtractTranslations(const Pose& other) const;
+
     std::pair<std::array<double, 3>, std::array<double, 4>> arrays() const;
     double getOriginTilt() const;
     double getOriginPan() const;

diff --git a/...ld/Perception/EntitiesPerceptionManager.h → ...tion/Managers/EntitiesPerceptionManager.h b/...ld/Perception/EntitiesPerceptionManager.h → ...tion/Managers/EntitiesPerceptionManager.h
@@ -7,7 +7,7 @@
 
 #include "overworld/BasicTypes/Entity.h"
 #include "overworld/Bullet/BulletClient.h"
-#include "overworld/Perception/PerceptionModuleBase.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
 
 #include "overworld/Utility/ShellDisplay.h"
 
@@ -41,7 +41,7 @@ class EntitiesPerceptionManager
     BulletClient* bullet_client_;
 
     bool shouldRun();
-    virtual void getPercepts(const std::map<std::string, T>& percepts);
+    virtual void getPercepts( std::map<std::string, T>& percepts);
     virtual void reasoningOnUpdate() {}
 
     void updateEntityPose(T* entity, const Pose& pose, const ros::Time& stamp);
@@ -73,7 +73,7 @@ void EntitiesPerceptionManager<T>::addPerceptionModule(const std::string& module
 template<typename T>
 PerceptionModuleBase_<T>* EntitiesPerceptionManager<T>::getPerceptionModule(const std::string& module_name)
 {
-    if(perception_modules_.find(module_name) == perception_modules_.end())
+    if(perception_modules_.find(module_name) != perception_modules_.end())
         return perception_modules_[module_name];
     else
         return nullptr;
@@ -150,7 +150,7 @@ bool EntitiesPerceptionManager<T>::shouldRun()
 }
 
 template<typename T>
-void EntitiesPerceptionManager<T>::getPercepts(const std::map<std::string, T>& percepts)
+void EntitiesPerceptionManager<T>::getPercepts( std::map<std::string, T>& percepts)
 {
     // This implementation only has test purposes
     for(auto& percept : percepts)
@@ -178,7 +178,7 @@ bool EntitiesPerceptionManager<T>::update()
 
     for(const auto& module : perception_modules_)
         if(module.second->isActivated() && module.second->hasBeenUpdated())
-            module.second->accessPercepts([this](const std::map<std::string, T>& percepts){ this->getPercepts(percepts); });
+            module.second->accessPercepts([this](std::map<std::string, T>& percepts){ this->getPercepts(percepts); });
 
     reasoningOnUpdate();
 
@@ -215,7 +215,9 @@ void EntitiesPerceptionManager<T>::addToBullet(T* entity)
                                                               entity->getShape().color[1],
                                                               entity->getShape().color[2],
                                                               1});
-            collision_id = bullet_client_->createCollisionShapeBox(entity->getShape().scale);
+            collision_id = bullet_client_->createCollisionShapeBox({entity->getShape().scale[0] / 2.,
+                                                                    entity->getShape().scale[1] / 2.,
+                                                                    entity->getShape().scale[2] / 2.});
             break;
         }
         case SHAPE_SPEHERE:
@@ -230,14 +232,14 @@ void EntitiesPerceptionManager<T>::addToBullet(T* entity)
         }
         case SHAPE_CYLINDER:
         {
-            visual_id = bullet_client_->createVisualShapeCylinder(entity->getShape().scale[0],
-                                                                  entity->getShape().scale[1],
+            visual_id = bullet_client_->createVisualShapeCylinder(std::min(entity->getShape().scale[0], entity->getShape().scale[1]) / 2.,
+                                                                  entity->getShape().scale[2],
                                                                   {entity->getShape().color[0],
                                                                    entity->getShape().color[1],
                                                                    entity->getShape().color[2],
                                                                    1});
-            collision_id = bullet_client_->createCollisionShapeCylinder(entity->getShape().scale[0],
-                                                                        entity->getShape().scale[1]);
+            collision_id = bullet_client_->createCollisionShapeCylinder(std::min(entity->getShape().scale[0], entity->getShape().scale[1]) / 2.,
+                                                                        entity->getShape().scale[2]);
             break;
         }
         case SHAPE_MESH:

diff --git a/...orld/Perception/HumansPerceptionManager.h → ...eption/Managers/HumansPerceptionManager.h b/...orld/Perception/HumansPerceptionManager.h → ...eption/Managers/HumansPerceptionManager.h
@@ -1,7 +1,7 @@
 #ifndef OWDS_HUMANSPERCEPTIONMANAGER_H
 #define OWDS_HUMANSPERCEPTIONMANAGER_H
 
-#include "overworld/Perception/EntitiesPerceptionManager.h"
+#include "overworld/Perception/Managers/EntitiesPerceptionManager.h"
 #include "overworld/BasicTypes/BodyPart.h"
 #include "overworld/BasicTypes/Agent.h"
 
@@ -19,7 +19,7 @@ class HumansPerceptionManager : public EntitiesPerceptionManager<BodyPart>
 private:
   std::map<std::string, Agent*> agents_;
 
-  void getPercepts(const std::map<std::string, BodyPart>& percepts) override;
+  void getPercepts( std::map<std::string, BodyPart>& percepts) override;
 
   void UpdateAgent(BodyPart* body_part);
 };

diff --git a/...rld/Perception/ObjectsPerceptionManager.h → ...ption/Managers/ObjectsPerceptionManager.h b/...rld/Perception/ObjectsPerceptionManager.h → ...ption/Managers/ObjectsPerceptionManager.h
@@ -1,7 +1,7 @@
 #ifndef OWDS_ObjectsPerceptionManager_H
 #define OWDS_ObjectsPerceptionManager_H
 
-#include "overworld/Perception/EntitiesPerceptionManager.h"
+#include "overworld/Perception/Managers/EntitiesPerceptionManager.h"
 #include "overworld/BasicTypes/Agent.h"
 #include "overworld/BasicTypes/Object.h"
 
@@ -22,7 +22,7 @@ class ObjectsPerceptionManager : public EntitiesPerceptionManager<Object>
   std::unordered_set<std::string> false_ids_to_be_merged_;
   std::map<std::string, std::string> merged_ids_;
 
-  void getPercepts(const std::map<std::string, Object>& percepts) override;
+  void getPercepts( std::map<std::string, Object>& percepts) override;
   void reasoningOnUpdate() override;
 
   std::vector<PointOfInterest> getPoisInFov(Object* object);
@@ -31,6 +31,8 @@ class ObjectsPerceptionManager : public EntitiesPerceptionManager<Object>
   std::unordered_set<int> getObjectsInCamera();
 
   void mergeFalseIdData();
+
+  void getObjectBoundingBox(Object* object);
 };
 
 } // namespace owds

diff --git a/...orld/Perception/RobotsPerceptionManager.h → ...eption/Managers/RobotsPerceptionManager.h b/...orld/Perception/RobotsPerceptionManager.h → ...eption/Managers/RobotsPerceptionManager.h
@@ -1,7 +1,7 @@
 #ifndef OWDS_ROBOTSPERCEPTIONMANAGER_H
 #define OWDS_ROBOTSPERCEPTIONMANAGER_H
 
-#include "overworld/Perception/EntitiesPerceptionManager.h"
+#include "overworld/Perception/Managers/EntitiesPerceptionManager.h"
 #include "overworld/BasicTypes/BodyPart.h"
 #include "overworld/BasicTypes/Agent.h"
 
@@ -19,7 +19,7 @@ class RobotsPerceptionManager : public EntitiesPerceptionManager<BodyPart>
 private:
   std::map<std::string, Agent*> agents_;
 
-  void getPercepts(const std::map<std::string, BodyPart>& percepts) override;
+  void getPercepts( std::map<std::string, BodyPart>& percepts) override;
 
   void UpdateAgent(BodyPart* body_part);
 };

diff --git a/...dalities/HumansEmulatedPerceptionModule.h → ...sModules/HumansEmulatedPerceptionModule.h b/...dalities/HumansEmulatedPerceptionModule.h → ...sModules/HumansEmulatedPerceptionModule.h
@@ -2,7 +2,7 @@
 #define OWDS_HUMANSEMULATEDPERCEPTIONMODULE_H
 
 #include "overworld/BasicTypes/BodyPart.h"
-#include "overworld/Perception/PerceptionModuleBase.h"
+#include "overworld/Perception/Modules/PerceptionModuleBase.h"
 
 #include <vector>