diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ccd6ce..96eca29 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/package.xml b/package.xml
index b900115..3371435 100644
--- a/package.xml
+++ b/package.xml
@@ -1,45 +1,15 @@
teleoperation
- 0.0.0
+ 0.1.0
The semester project package
-
-
-
- hrs_e
+ HRS_E
+ MIT
-
-
-
- TODO
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
catkin
+
cv_bridge
image_transport
roscpp
@@ -49,6 +19,7 @@
aruco
message_generation
actionlib_msgs
+
cv_bridge
image_transport
roscpp
@@ -59,10 +30,7 @@
message_runtime
actionlib_msgs
-
-
-
-
+
diff --git a/script/calibrator.py b/script/calibrator.py
index 32b4f97..d2bd9a3 100755
--- a/script/calibrator.py
+++ b/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')
diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp
index 3a0e219..6dd64ea 100644
--- a/src/rviz_human.cpp
+++ b/src/rviz_human.cpp
@@ -3,9 +3,11 @@
#include
#include
#include
+#include
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());
+
// 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{
- aruco_0.lookupTransform("/odom", "/Aruco_0_frame",ros::Time(0), aruco_0_tf);
+ try {
+ 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]);