implemented YAML parsing for rviz_human

CMakeLists.txt

@@ -54,4 +54,4 @@ add_executable(aruco_detector src/aruco_detector.cpp)
 add_executable(rviz_human src/rviz_human.cpp)
 
 target_link_libraries(aruco_detector ${catkin_LIBRARIES})
-target_link_libraries(rviz_human ${catkin_LIBRARIES})
+target_link_libraries(rviz_human ${catkin_LIBRARIES} yaml-cpp)

package.xml

@@ -1,45 +1,15 @@
 <?xml version="1.0"?>
 <package>
   <name>teleoperation</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
   <description>The semester project package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <maintainer email="pavel.lutskov@tum.de">HRS_E</maintainer>
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="hrs_e@todo.todo">hrs_e</maintainer>
 
+  <license>MIT</license>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/project_aruco</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
+
   <build_depend>cv_bridge</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>roscpp</build_depend>
@@ -49,6 +19,7 @@
   <build_depend>aruco</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>actionlib_msgs</build_depend>
+
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>roscpp</run_depend>
@@ -59,10 +30,7 @@
   <run_depend>message_runtime</run_depend>
   <run_depend>actionlib_msgs</run_depend>
 
-
-  <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <!-- Other tools can request additional information be placed here -->
-
   </export>
+
 </package>

script/calibrator.py

@@ -49,6 +49,22 @@ def calibration():
     except Exception:
         pass
 
+    tts.say('Now place the markers on the corresponding shoulders')
+    tts.say('Make sure the markers are detected')
+    tts.say('Then say start')
+    client.send_goal(RequestSpeechGoal(['start']))
+    client.wait_for_result()
+
+    try:
+        lsh, _ = ll.lookupTransform('Aruco_0_frame',
+                                    'Aruco_1_frame',
+                                    rospy.Time(0))
+        rsh, _ = ll.lookupTransform('Aruco_0_frame',
+                                    'Aruco_2_frame',
+                                    rospy.Time(0))
+    except Exception:
+        pass
+
     tts.say('Now rotate to your right')
     tts.say('So that marker is still detected')
     tts.say('Then say start')

src/rviz_human.cpp

@@ -3,9 +3,11 @@
 #include <ros/ros.h>
 #include <visualization_msgs/Marker.h>
 #include <tf/transform_listener.h>
+#include <yaml-cpp/yaml.h>
 
 int main( int argc, char** argv )
 {
+
     ros::init(argc, argv, "rviz_human");
     ros::NodeHandle n;
     ros::Rate r(100);
@@ -16,13 +18,19 @@ int main( int argc, char** argv )
     tf::TransformListener aruco_1;
     tf::TransformListener aruco_2;
 
-    // suppose we already have the position of the center marker in the correct frame
+    // suppose we already have the position of the center marker
+    // in the correct frame
 
     geometry_msgs::Point torso;
     geometry_msgs::Point l_hand;
     geometry_msgs::Point r_hand;
     geometry_msgs::Point base_position;
 
+    // !!! YAML PARSING WORKS LIKE THIS !!!
+    // YAML::Node config = YAML::LoadFile(
+            // "/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");
+    // ROS_INFO("%lf", config["arm"].as<double>());
+
     // Set human to starting postion
     torso.x = -1;
     torso.y = 0;
@@ -50,18 +58,21 @@ int main( int argc, char** argv )
         tf::StampedTransform aruco_1_tf;
         tf::StampedTransform aruco_2_tf;
 
-        try{
+        try {
-            aruco_0.lookupTransform("/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
+            aruco_0.lookupTransform("/odom", "/Aruco_0_frame",
+                    ros::Time(0), aruco_0_tf);
             ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
             ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
             ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);
 
-            aruco_1.lookupTransform("/odom", "/Aruco_1_frame",ros::Time(0), aruco_1_tf);
+            aruco_1.lookupTransform("/odom", "/Aruco_1_frame",
+                    ros::Time(0), aruco_1_tf);
             ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
             ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
             ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);
 
-            aruco_2.lookupTransform("/odom", "/Aruco_2_frame",ros::Time(0), aruco_2_tf);
+            aruco_2.lookupTransform("/odom", "/Aruco_2_frame",
+                    ros::Time(0), aruco_2_tf);
             ROS_INFO("0_2: %f",aruco_2_tf.getOrigin()[0]);
             ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]);
             ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]);