pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

implemented YAML parsing for rviz_human

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-09 10:22:06 +01:00
parent 8861d215c5
commit 2ec3ebaf79
4 changed files with 39 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -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)

44
package.xml
View File

@@ -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 -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="hrs_e@todo.todo">hrs_e</maintainer>
<maintainer email="pavel.lutskov@tum.de">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>

16
script/calibrator.py
View File

@@ -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')

19
src/rviz_human.cpp
View File

@@ -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;
@@ -51,17 +59,20 @@ int main( int argc, char** argv )
tf::StampedTransform aruco_2_tf;
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]);