Merge pull request #1 from theoh-io/pose_node

Pose node

src/loomo/launch/Loomo.launch

@@ -2,28 +2,33 @@
     <!-- Explanation in the README.md file -->
 
     <!-- GLOBAL PARAMETERS -->
-    <param name="ip_address" value="128.179.153.165" />
+    <param name="ip_address" value="128.179.159.159" />
     <!-- <param name="ip_address_neuro" value="192.168.0.152" /> -->
     <!-- <param name="abs_path_to_tools" value="/home/theo/Autonomous_driving_pipeline/src/loomo/scripts/tools" /> -->
     <param name="v_max" value="1" />
     <param name="wheel_base" value="0.57" />
-    <param name="speed" value="0.7" />
+    <param name="speed" value="0.5" />
 
 
     <!-- PERCEPTION PARAMETERS: Default, Stark -->
     <param name="PERCEPTION_FUNCTION" value="Default" />
     <param name="model_perception_path" value="/home/theo/Autonomous_driving_pipeline/src/perception/scripts/Perception_Functions/saved_model.pth" />
-    <param name="dt_perception" value="0.06" />
+    <param name="dt_perception" value="0.2" />
     <param name="downscale" value="2" />
     <param name="detector_size" value="medium" />
     <param name="tracking_confidence" value="0.9" />
-    <param name="keypoints_activated" value="false" />
-    <param name="save_keypoints_vid" value="false" />
-    <param name="keypoints_logging" value="false" />
     <param name="visualization_percep" value="true" /> 
-    <param name="visualization_3D_activated" value="false" />
     <param name="verbose_percep" type="int" value= "2" /> <!-- level of verbose 0 to 4-->
 
+    <!-- POSE ESTIMATION PARAMETERS: Default, VideoPose3D -->
+    <param name="POSE_ESTIMATION_FUNCTION" value="Default" />
+    <param name="keypoints_activated" value="true" />
+    <param name="dt_pose_estimation" value="0.2" />
+    <param name="visualization_3D_activated" value="false" />
+    <param name="keypoints_logging" value="false" />
+    <param name="save_keypoints_vid" value="true" />
+    <param name="verbose_keypoints" value="true" />
+
 
     <!-- ROBOT STATE PARAMETERS: Default, EPFL_Driverless -->
     <param name="ROBOT_STATE_FUNCTION" value="Default" />
@@ -60,7 +65,7 @@
 
     <!-- CONTROL PARAMETERS: Default, EPFL_Driverless -->
     <param name="CONTROL_FUNCTION" value="Default" />
-    <param name="dt_control" value="0.2" />
+    <param name="dt_control" value="0.25" />
     <param name="time_horizon_control" value="1.2" />
     <param name="n_states" value="3" />
     <param name="verbose_ctrl" value="2" />
@@ -74,6 +79,10 @@
     <node pkg="perception" type="perception.py" name="perception" output="screen">
     </node>
 
+    <!-- POSE ESTIMATION NODE -->
+    <node pkg="pose_estimation" type="pose_estimation.py" name="pose_estimation" output="screen">
+    </node>
+
     <!-- STATE ESTIMATION NODE -->
     <node pkg="state_estimation" type="robot_state.py" name="robot_state" output="screen">
     </node>

src/loomo/scripts/tools/classconverter.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 # VITA, EPFL
-from msg_types.msg import PositionArray, Position, TrajectoryArray, State, StateArray, ControlCmd
+from msg_types.msg import PositionArray, Position, TrajectoryArray, State, StateArray, ControlCmd, Bbox
 from copy import deepcopy
 
 
@@ -139,4 +139,16 @@ def PositionArray2TrajectoryArraylist(data):
     if len(traj_objects) == 0:
         traj_objects = []
 
-    return traj_objects
+    return traj_objects
+
+def list2Bbox(data):
+    bbox = Bbox()
+    bbox.xcenter = data[0]
+    bbox.ycenter = data[1]
+    bbox.width = data[2]
+    bbox.height = data[3]
+    return bbox
+
+def Bbox2list(bbox):
+    bbox_list=[bbox.xcenter, bbox.ycenter, bbox.width, bbox.height]
+    return bbox_list

src/msg_types/CMakeLists.txt

@@ -51,6 +51,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msg
    Position.msg
    PositionArray.msg
    TrajectoryArray.msg
+   Bbox.msg
  )
 
 ## Generate services in the 'srv' folder

src/msg_types/msg/Bbox.msg

@@ -0,0 +1,6 @@
+time stamp
+float32 xcenter
+float32 ycenter
+float32 width
+float32 height
+

src/perception/package.xml

@@ -46,6 +46,9 @@
      <exec_depend>message_runtime</exec_depend> 
      <depend>geometry_msgs</depend>
      <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>opencv2</depend>
+  <depend>cv_bridge</depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->