From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 26 Dec 2018 06:01:50 +0000 (-0200)
Subject: Add publish WAM Cartesian pose.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=eed20975c3c35606c756b68b681b757f9e80df81;p=ufrgs_wam.git

Add publish WAM Cartesian pose.
---

diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp
index 6752e89..b645361 100644
--- a/wam_node_sim/src/wam_node_sim.cpp
+++ b/wam_node_sim/src/wam_node_sim.cpp
@@ -611,7 +611,7 @@ template<size_t DOF>
     jt_type jt = wam.getJointTorques();
     jv_type jv = wam.getJointVelocities();
     cp_type cp_pub = wam.getToolPosition();
-//    Eigen::Quaterniond to_pub = wam.getToolOrientation();
+    Eigen::Quaterniond to_pub = wam.getToolOrientation();
 
     //publishing sensor_msgs/JointState to wam/joint_states
     for (size_t i = 0; i < DOF; i++)
@@ -626,7 +626,7 @@ template<size_t DOF>
 
     //publishing geometry_msgs/PoseStamed to wam/pose
     wam_pose.header.stamp = ros::Time::now();
-/*
+
     wam_pose.pose.position.x = cp_pub[0];
     wam_pose.pose.position.y = cp_pub[1];
     wam_pose.pose.position.z = cp_pub[2];
@@ -634,7 +634,7 @@ template<size_t DOF>
     wam_pose.pose.orientation.x = to_pub.x();
     wam_pose.pose.orientation.y = to_pub.y();
     wam_pose.pose.orientation.z = to_pub.z();
-*/
+
     wam_pose_pub.publish(wam_pose);
   }