diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e5dba7..cda7e17 100644 --- 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 index a94ebc2..3173a07 100644 --- 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( - "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( - "fts/" + device + "/tared_wrench", FCAT_PUB_QUEUE_SIZE); - break; - } - */ + std::string device = (*dev_state_ptr)->name; + fts_raw_pub_map_[device] = + this->create_publisher( + "fts/" + device + "/raw_wrench", FCAT_PUB_QUEUE_SIZE); + fts_tared_pub_map_[device] = + this->create_publisher( + "fts/" + device + "/tared_wrench", FCAT_PUB_QUEUE_SIZE); + } module_state_pub_ = this->create_publisher( "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 index 63a43e2..aeb0556 100644 --- 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; + 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::SharedPtr joint_state_pub_; rclcpp::Publisher::SharedPtr module_state_pub_; - rclcpp::Publisher::SharedPtr fts_raw_pub_; - rclcpp::Publisher::SharedPtr fts_tared_pub_; rclcpp::Publisher::SharedPtr actuator_pub_; rclcpp::Publisher::SharedPtr egd_pub_; @@ -277,6 +277,8 @@ class Fcat: public rclcpp::Node{ rclcpp::Publisher::SharedPtr conditional_pub_; rclcpp::Publisher::SharedPtr schmitt_trigger_pub_; + std::unordered_map fts_raw_pub_map_; + std::unordered_map fts_tared_pub_map_; ////////////////////