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

21
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;
@@ -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]);