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 (file)
--- 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);
   }