From eed20975c3c35606c756b68b681b757f9e80df81 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 26 Dec 2018 04:01:50 -0200 Subject: [PATCH] Add publish WAM Cartesian pose. --- wam_node_sim/src/wam_node_sim.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 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 //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 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); } -- 2.12.0