added fts wrench publishing; ATI FTS support via fastcat v0.4.8

nasa-jpl · Feb 21, 2021 · dcefb9c · dcefb9c
1 parent 74027cc
commit dcefb9c
Show file tree

Hide file tree

Showing 3 changed files with 56 additions and 17 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.11)
 project(fcat
-    VERSION 0.5.1
+    VERSION 0.5.2
     LANGUAGES C CXX
     DESCRIPTION "ROS2 Fastcat Node"
     )
@@ -23,7 +23,7 @@ endif()
 include(FetchContent)
 FetchContent_Declare(fastcat
     GIT_REPOSITORY https://github.com/nasa-jpl/fastcat.git
-    GIT_TAG v0.4.6
+    GIT_TAG v0.4.8
     )
 FetchContent_MakeAvailable(fastcat)
 

diff --git a/src/fcat.cpp b/src/fcat.cpp
@@ -277,20 +277,20 @@ void Fcat::InitializePublishersAndMessages(){
     signal_generator_states_msg_.states.resize(vec_state_ptrs.size());
   }
 
-  /* TODO handle similar to joint_states
-     case fastcat::FTS_STATE: {
-     std::string device = it->name.c_str();
-     fts_raw_pub_[device] = 
-     this->create_publisher<geometry_msgs::msg::Wrench>(
-     "fts/" + device + "/raw_wrench", FCAT_PUB_QUEUE_SIZE);
+
+  // FTS, AtiFts, VirtualFts
+  vec_state_ptrs = device_type_vec_map_[fastcat::FTS_STATE];
+  for(auto dev_state_ptr = vec_state_ptrs.begin(); dev_state_ptr != vec_state_ptrs.end(); dev_state_ptr++){
 
-     fts_tared_pub_[device] = 
-     this->create_publisher<geometry_msgs::msg::Wrench>(
-     "fts/" + device + "/tared_wrench", FCAT_PUB_QUEUE_SIZE);
-     break;
-     }
-     */
+     std::string device = (*dev_state_ptr)->name;
+     fts_raw_pub_map_[device] = 
+       this->create_publisher<geometry_msgs::msg::Wrench>(
+         "fts/" + device + "/raw_wrench", FCAT_PUB_QUEUE_SIZE);
 
+     fts_tared_pub_map_[device] = 
+       this->create_publisher<geometry_msgs::msg::Wrench>(
+         "fts/" + device + "/tared_wrench", FCAT_PUB_QUEUE_SIZE);
+   }
 
   module_state_pub_ = this->create_publisher<fcat_msgs::msg::ModuleState>(
       "state/module_state", FCAT_PUB_QUEUE_SIZE);
@@ -426,6 +426,8 @@ void Fcat::Process(){
 
   PublishModuleState();
 
+  PublishFtsStates();
+
   PublishActuatorStates();
   PublishEgdStates();
   PublishEl2124States();
@@ -456,6 +458,41 @@ void Fcat::PublishModuleState(){
   module_state_pub_->publish(module_state_msg);
 }
 
+void Fcat::PublishFtsStates(){
+
+  // FTS, AtiFts, VirtualFts
+  auto vec_state_ptrs = device_type_vec_map_[fastcat::FTS_STATE];
+
+  for(auto dev_state_ptr = vec_state_ptrs.begin(); dev_state_ptr != vec_state_ptrs.end(); dev_state_ptr++){
+  auto wrench_msg = geometry_msgs::msg::Wrench();
+
+    std::string device = (*dev_state_ptr)->name;
+    fastcat::FtsState fts_state = (*dev_state_ptr)->fts_state;
+
+    wrench_msg.force.x = fts_state.raw_fx;
+    wrench_msg.force.y = fts_state.raw_fy;
+    wrench_msg.force.z = fts_state.raw_fz;
+
+    wrench_msg.torque.x = fts_state.raw_tx;
+    wrench_msg.torque.y = fts_state.raw_ty;
+    wrench_msg.torque.z = fts_state.raw_tz;
+
+    fts_raw_pub_map_[device]->publish(wrench_msg);
+
+
+    wrench_msg.force.x = fts_state.tared_fx;
+    wrench_msg.force.y = fts_state.tared_fy;
+    wrench_msg.force.z = fts_state.tared_fz;
+
+    wrench_msg.torque.x = fts_state.tared_tx;
+    wrench_msg.torque.y = fts_state.tared_ty;
+    wrench_msg.torque.z = fts_state.tared_tz;
+
+    fts_tared_pub_map_[device]->publish(wrench_msg);
+   }
+
+}
+
 void Fcat::PublishActuatorStates(){
 
   auto act_state_vec = device_type_vec_map_[fastcat::ACTUATOR_STATE];

diff --git a/src/fcat.hpp b/src/fcat.hpp
@@ -61,6 +61,8 @@
 const unsigned int FCAT_PUB_QUEUE_SIZE = 32;
 const unsigned int FCAT_SUB_QUEUE_SIZE = 32;
 
+using WrenchPublisher = rclcpp::Publisher<geometry_msgs::msg::Wrench>;
+
 inline double fcat_get_time_sec()
 {
   struct timeval tv;
@@ -131,7 +133,7 @@ class Fcat: public rclcpp::Node{
   void UpdateStateMap();
 
   void PublishModuleState();
-  //void PublishWrenches();
+  void PublishFtsStates();
 
   void PublishActuatorStates();
   void PublishEgdStates();
@@ -257,8 +259,6 @@ class Fcat: public rclcpp::Node{
 
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_; 
   rclcpp::Publisher<fcat_msgs::msg::ModuleState>::SharedPtr  module_state_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::Wrench>::SharedPtr   fts_raw_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::Wrench>::SharedPtr   fts_tared_pub_;
 
   rclcpp::Publisher<fcat_msgs::msg::ActuatorStates>::SharedPtr actuator_pub_;
   rclcpp::Publisher<fcat_msgs::msg::EgdStates>::SharedPtr      egd_pub_;
@@ -277,6 +277,8 @@ class Fcat: public rclcpp::Node{
   rclcpp::Publisher<fcat_msgs::msg::ConditionalStates>::SharedPtr conditional_pub_;
   rclcpp::Publisher<fcat_msgs::msg::SchmittTriggerStates>::SharedPtr schmitt_trigger_pub_;
 
+  std::unordered_map<std::string, WrenchPublisher::SharedPtr>  fts_raw_pub_map_;
+  std::unordered_map<std::string, WrenchPublisher::SharedPtr>  fts_tared_pub_map_;
 
 
   ////////////////////