Merge pull request #73 from TAMS-Group/pr-kinetic-add-ft-frame

add frame_id to f/t wrench messages
PR2 · Feb 12, 2019 · de67121 · de67121
2 parents 825f997 + dd61ecb
commit de67121
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/ethercat_hardware/src/wg06.cpp b/ethercat_hardware/src/wg06.cpp
@@ -734,6 +734,9 @@ bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *
   // Make room in data structure for more f/t samples
   ft_state.samples_.resize(usable_samples);
 
+  // add side "l" or "r" to frame_id
+  string ft_link_id = string(actuator_info_.name_).substr(0,1) + "_force_torque_link";
+
   // If any f/t channel is overload or the sampling rate is bad, there is an error.
   ft_state.good_ = ( (!ft_sampling_rate_error_) && 
                      (ft_overload_flags_ == 0) && 
@@ -789,6 +792,7 @@ bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *
   if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
   {
     ft_publisher_->msg_.header.stamp = current_time;
+    ft_publisher_->msg_.header.frame_id = ft_link_id;
     ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
     ft_publisher_->unlockAndPublish();
   }