├── .clang-format ├── .gitignore ├── netft_interfaces ├── CMakeLists.txt ├── msg │ └── Cancel.msg ├── package.xml └── srv │ ├── GetDouble.srv │ ├── SetBias.srv │ ├── SetFilter.srv │ ├── SetMax.srv │ ├── SetThreshold.srv │ ├── SetToolData.srv │ ├── StartSim.srv │ └── StopSim.srv └── netft_utils ├── CMakeLists.txt ├── License.txt ├── README.md ├── config └── control_params.yaml ├── include ├── lpfilter.h ├── netft_utils.h ├── netft_utils │ ├── netft_hardware_interface.hpp │ ├── netft_rdt_driver.h │ └── visibility_control.h └── netft_utils_lean.h ├── launch └── netft.launch.py ├── netft_hardware_interface_plugin.xml ├── package.xml ├── src ├── lpfilter.cpp ├── netft_hardware_interface.cpp ├── netft_node.cpp ├── netft_rdt_driver.cpp ├── netft_utils.cpp ├── netft_utils_lean.cpp └── netft_utils_sim.cpp └── urdf ├── example_description.urdf.xacro └── ros2_control.xacro /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | 5 | AccessModifierOffset: -2 6 | AlignAfterOpenBracket: AlwaysBreak 7 | BraceWrapping: 8 | AfterClass: true 9 | AfterFunction: true 10 | AfterNamespace: true 11 | AfterStruct: true 12 | AfterEnum: true 13 | BreakBeforeBraces: Custom 14 | ColumnLimit: 100 15 | ConstructorInitializerIndentWidth: 0 16 | ContinuationIndentWidth: 2 17 | DerivePointerAlignment: false 18 | PointerAlignment: Middle 19 | ReflowComments: false 20 | ... 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | **/.vscode/ 2 | -------------------------------------------------------------------------------- /netft_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(netft_interfaces) 3 | 4 | if (NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif () 7 | 8 | find_package(ament_cmake REQUIRED) 9 | find_package(rosidl_default_generators REQUIRED) 10 | find_package(control_msgs REQUIRED) 11 | find_package(action_msgs REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(common_interfaces REQUIRED) 14 | find_package(geometry_msgs REQUIRED) 15 | 16 | include_directories(msg srv) 17 | 18 | rosidl_generate_interfaces(${PROJECT_NAME} 19 | msg/Cancel.msg 20 | srv/GetDouble.srv 21 | srv/SetBias.srv 22 | srv/SetFilter.srv 23 | srv/SetMax.srv 24 | srv/SetThreshold.srv 25 | srv/SetToolData.srv 26 | srv/StartSim.srv 27 | srv/StopSim.srv 28 | DEPENDENCIES builtin_interfaces geometry_msgs std_msgs common_interfaces 29 | ) 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /netft_interfaces/msg/Cancel.msg: -------------------------------------------------------------------------------- 1 | bool cancel -------------------------------------------------------------------------------- /netft_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | netft_interfaces 4 | 0.0.0 5 | The netft_interfaces package 6 | 7 | Jacob Odle 8 | Jacob Odle 9 | TODO 10 | 11 | 12 | rosidl_default_generators 13 | rosidl_default_runtime 14 | rosidl_interface_packages 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /netft_interfaces/srv/GetDouble.srv: -------------------------------------------------------------------------------- 1 | bool ignore 2 | --- 3 | float64 weight 4 | -------------------------------------------------------------------------------- /netft_interfaces/srv/SetBias.srv: -------------------------------------------------------------------------------- 1 | bool bias 2 | float64 force 3 | float64 torque 4 | --- 5 | bool success 6 | -------------------------------------------------------------------------------- /netft_interfaces/srv/SetFilter.srv: -------------------------------------------------------------------------------- 1 | bool filter 2 | float64 t 3 | float64 cutoff_frequency 4 | --- 5 | -------------------------------------------------------------------------------- /netft_interfaces/srv/SetMax.srv: -------------------------------------------------------------------------------- 1 | float64 force 2 | float64 torque 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /netft_interfaces/srv/SetThreshold.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/WrenchStamped data 2 | --- 3 | bool success 4 | -------------------------------------------------------------------------------- /netft_interfaces/srv/SetToolData.srv: -------------------------------------------------------------------------------- 1 | float64 mass 2 | float64 com 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /netft_interfaces/srv/StartSim.srv: -------------------------------------------------------------------------------- 1 | int32 dim 2 | int32 type 3 | float64 slope 4 | float64 force 5 | --- 6 | -------------------------------------------------------------------------------- /netft_interfaces/srv/StopSim.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | 4 | -------------------------------------------------------------------------------- /netft_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(netft_utils) 3 | 4 | if (NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif () 7 | 8 | if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif () 11 | 12 | set(THIS_PACKAGE_INCLUDE_DEPENDS 13 | ament_cmake 14 | rclcpp 15 | rclcpp_lifecycle 16 | std_msgs 17 | common_interfaces 18 | hardware_interface 19 | geometry_msgs 20 | pluginlib 21 | tf2 22 | tf2_ros 23 | netft_interfaces 24 | ) 25 | foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 26 | find_package(${Dependency} REQUIRED) 27 | endforeach() 28 | find_package(Boost REQUIRED COMPONENTS system thread program_options) 29 | 30 | include_directories(src include /usr/include/ ${Boost_INCLUDE_DIRS}) 31 | 32 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 33 | 34 | # Library for actually talking to the sensor 35 | add_library(netft_rdt_driver SHARED 36 | src/netft_rdt_driver.cpp 37 | ) 38 | target_compile_features(netft_rdt_driver PUBLIC cxx_std_17) 39 | target_include_directories(netft_rdt_driver 40 | PUBLIC 41 | $ 42 | $) 43 | ament_target_dependencies(netft_rdt_driver ${THIS_PACKAGE_INCLUDE_DEPENDS}) 44 | target_link_libraries(netft_rdt_driver ${Boost_LIBRARIES}) 45 | 46 | # Causes the visibility macros to use dllexport rather than dllimport, 47 | # which is appropriate when building the dll but not consuming it. 48 | target_compile_definitions(netft_rdt_driver PRIVATE "NETFT_RDT_DRIVER_BUILDING_LIBRARY") 49 | 50 | # Executable to enable a ROS node bringup 51 | add_executable(netft_node src/netft_node.cpp) 52 | target_include_directories(netft_node PUBLIC 53 | $ 54 | $) 55 | target_link_libraries(netft_node netft_rdt_driver) 56 | 57 | # Library for bringup with ros2_controls hardware_interface 58 | add_library( 59 | netft_hardware_interface 60 | SHARED 61 | src/netft_hardware_interface.cpp 62 | ) 63 | target_compile_features(netft_hardware_interface PUBLIC cxx_std_17) 64 | target_include_directories(netft_hardware_interface PUBLIC 65 | $ 66 | $ 67 | ) 68 | target_link_libraries(netft_hardware_interface PUBLIC netft_rdt_driver) 69 | ament_target_dependencies( 70 | netft_hardware_interface PUBLIC 71 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 72 | ) 73 | 74 | # Causes the visibility macros to use dllexport rather than dllimport, 75 | # which is appropriate when building the dll but not consuming it. 76 | target_compile_definitions(netft_hardware_interface PRIVATE "NETFT_HARDWARE_INTERFACE_BUILDING_DLL") 77 | 78 | # Export hardware plugins 79 | pluginlib_export_plugin_description_file(hardware_interface netft_hardware_interface_plugin.xml) 80 | 81 | install(TARGETS netft_hardware_interface 82 | EXPORT export_netft_hardware_interface 83 | ARCHIVE DESTINATION lib 84 | LIBRARY DESTINATION lib 85 | RUNTIME DESTINATION bin 86 | ) 87 | 88 | install( 89 | TARGETS netft_node 90 | DESTINATION lib/${PROJECT_NAME} 91 | ) 92 | 93 | install(TARGETS netft_rdt_driver 94 | EXPORT export_netft_rdt_driver 95 | ARCHIVE DESTINATION lib 96 | LIBRARY DESTINATION lib 97 | RUNTIME DESTINATION bin 98 | ) 99 | 100 | install(DIRECTORY include/ DESTINATION include) 101 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 102 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 103 | install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) 104 | 105 | ament_export_targets(export_netft_rdt_driver HAS_LIBRARY_TARGET) 106 | ament_export_targets(export_netft_hardware_interface HAS_LIBRARY_TARGET) 107 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 108 | 109 | ament_package() 110 | -------------------------------------------------------------------------------- /netft_utils/License.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Los Alamos National Security, LLC 2 | All rights reserved. 3 | Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL. 4 | 5 | Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 9 | 10 | THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | -------------------------------------------------------------------------------- /netft_utils/README.md: -------------------------------------------------------------------------------- 1 | C++ class and ROS node for ATI force/torque sensors connected to a Netbox. Includes gravity compensation and transformations. 2 | 3 | # Usage 4 | There are two main ways to use this package: 5 | 1. With a direct ROS node 6 | 1. Using `ros2_control` hardware interface and a `force_torque_broadcaster` 7 | 8 | To launch the direct node, run the following with the correct IP address 9 | ```sh 10 | ros2 run netft_utils netft_node --address WWW.XXX.YYY.ZZZ 11 | ``` 12 | 13 | To run an example launch for `ros2_control` run the following with option namespace 14 | ```sh 15 | ros2 launch netft_utils netft.launch.py node_namespace:="test_ns/" 16 | ``` 17 | -------------------------------------------------------------------------------- /netft_utils/config/control_params.yaml: -------------------------------------------------------------------------------- 1 | $(var node_namespace)controller_manager: 2 | ros__parameters: 3 | update_rate: 500 # Hz 4 | 5 | force_torque_sensor_broadcaster: 6 | type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster 7 | 8 | 9 | $(var node_namespace)force_torque_sensor_broadcaster: 10 | ros__parameters: 11 | sensor_name: netft_sensor 12 | state_interface_names: 13 | - force.x 14 | - force.y 15 | - force.z 16 | - torque.x 17 | - torque.y 18 | - torque.z 19 | frame_id: netft_link 20 | -------------------------------------------------------------------------------- /netft_utils/include/lpfilter.h: -------------------------------------------------------------------------------- 1 | #ifndef LP_FILTER_H 2 | #define LP_FILTER_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class LPFilter 9 | { 10 | public: 11 | LPFilter(double deltaT, double cutoffFrequency, int numElements); 12 | bool update(std::vector input, std::vector& output); 13 | 14 | private: 15 | bool initialized; 16 | int noElements; 17 | std::vector in1, in2, out1, out2; 18 | double omega_a; 19 | double a0, a1, a2, b1, b2; 20 | }; 21 | 22 | #endif -------------------------------------------------------------------------------- /netft_utils/include/netft_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef NET_FT_UTILS_H 2 | #define NET_FT_UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | /** 21 | * This program takes force/torque data and applies transforms to usable data 22 | */ 23 | 24 | namespace netft_utils 25 | { 26 | 27 | class NetftUtils : rclcpp::Node 28 | { 29 | public: 30 | NetftUtils(); 31 | ~NetftUtils(); 32 | 33 | void initialize(); 34 | void setUserInput(std::string world, std::string ft, double force, double torque); 35 | void update(); 36 | rclcpp::Logger getLog(); 37 | 38 | private: 39 | 40 | //LPFilter 41 | LPFilter* lp; // Filter 42 | bool isFilterOn; 43 | double deltaTFilter; 44 | double cutoffFrequency; 45 | bool newFilter; 46 | 47 | // Transform listener 48 | tf2_ros::TransformListener* listener; 49 | tf2::BufferCore bufferCore; 50 | tf2::Stamped ft_to_world; // Transform from ft frame to world frame 51 | std::string world_frame; 52 | std::string ft_frame; 53 | 54 | // Wrenches used to hold force/torque and bias data 55 | geometry_msgs::msg::WrenchStamped bias; // Wrench containing the current bias data in tool frame 56 | geometry_msgs::msg::WrenchStamped weight_bias; // Wrench containing the bias at a measurement pose (to measure the weight) 57 | geometry_msgs::msg::WrenchStamped raw_data_world; // Wrench containing the current raw data from the netft sensor transformed into the world frame 58 | geometry_msgs::msg::WrenchStamped raw_data_tool; // Wrench containing the current raw data from the netft sensor in the tool frame 59 | geometry_msgs::msg::WrenchStamped tf_data_world; // Wrench containing the transformed (world frame) data with bias and threshold applied 60 | geometry_msgs::msg::WrenchStamped tf_data_tool; // Wrench containing the transformed (tool frame) data with bias and threshold applied 61 | geometry_msgs::msg::WrenchStamped zero_wrench; // Wrench of all zeros for convenience 62 | geometry_msgs::msg::WrenchStamped threshold; // Wrench containing thresholds 63 | 64 | double payloadWeight; // Used in gravity compensation 65 | double payloadLeverArm; // Used in gravity compensation. The z-coordinate to payload CoM (in sensor's raw frame) 66 | 67 | bool isBiased; // True if sensor is biased 68 | bool isNewBias; // True if sensor was biased this pass 69 | bool isNewGravityBias; // True if gravity compensation was applied this pass 70 | bool isGravityBiased; // True if gravity is compensated 71 | 72 | // Variables used to monitor FT violation and send a cancel move message 73 | netft_interfaces::msg::Cancel cancel_msg; 74 | static const int MAX_CANCEL = 5; // Number of times to send cancel message when max force is exceeded 75 | static const int MAX_WAIT = 100; // Number of cycles to wait after cancel message before sending again 76 | int cancel_count; // Counter for times to send cancel message when max force is exceeded 77 | int cancel_wait; // Counter of cycles to wait after cancel message before sending again 78 | double forceMaxB; // Default max force limit to send cancel when FT is biased 79 | double torqueMaxB; // Default max torque limit to send cancel when FT is biased 80 | double forceMaxU; // Default max force limit to send cancel when FT is unbiased 81 | double torqueMaxU; // Default max torque limit to send cancel when FT is unbiased 82 | 83 | // ROS subscribers 84 | rclcpp::Subscription::SharedPtr raw_data_sub; 85 | 86 | // ROS publishers 87 | rclcpp::Publisher::SharedPtr netft_raw_world_data_pub; 88 | rclcpp::Publisher::SharedPtr netft_world_data_pub; 89 | rclcpp::Publisher::SharedPtr netft_tool_data_pub; 90 | rclcpp::Publisher::SharedPtr netft_cancel_pub; 91 | 92 | //////////////// 93 | // ROS services 94 | //////////////// 95 | rclcpp::Service::SharedPtr bias_service; 96 | rclcpp::Service::SharedPtr gravity_comp_service; 97 | rclcpp::Service::SharedPtr set_max_service; 98 | rclcpp::Service::SharedPtr threshold_service; 99 | rclcpp::Service::SharedPtr weight_bias_service; 100 | rclcpp::Service::SharedPtr get_weight_service; 101 | rclcpp::Service::SharedPtr filter_service; 102 | 103 | //////////////////// 104 | // Callback methods 105 | //////////////////// 106 | 107 | // Runs when a new datapoint comes in 108 | void netftCallback(const geometry_msgs::msg::WrenchStamped::ConstPtr& data); 109 | 110 | // Set the readings from the sensor to zero at this instant and continue to apply the bias on future readings. 111 | // This doesn't account for gravity i.e. it will not change if the sensor's orientation changes. 112 | // Run this method when the sensor is stationary to avoid inertial effects. 113 | bool fixedOrientationBias(netft_interfaces::srv::SetBias::Request &req, netft_interfaces::srv::SetBias::Response &res); 114 | 115 | // Set the readings from the sensor to zero at this instant. 116 | // Calculate the payload's mass and center of mass so gravity can be compensated for, even as the sensor changes orientation. 117 | // It's assumed that the payload's center of mass is located on the sensor's central access. 118 | // Run this method when the sensor is stationary to avoid inertial effects. 119 | // It assumes the Z-axis of the World tf frame is up. 120 | bool compensateForGravity(netft_interfaces::srv::SetBias::Request &req, netft_interfaces::srv::SetBias::Response &res); 121 | 122 | bool setMax(netft_interfaces::srv::SetMax::Request &req, netft_interfaces::srv::SetMax::Response &res); 123 | bool setWeightBias(netft_interfaces::srv::SetBias::Request &req, netft_interfaces::srv::SetBias::Response &res); 124 | bool getWeight(netft_interfaces::srv::GetDouble::Request &req, netft_interfaces::srv::GetDouble::Response &res); 125 | bool setThreshold(netft_interfaces::srv::SetThreshold::Request &req, netft_interfaces::srv::SetThreshold::Response &res); 126 | bool setFilter(netft_interfaces::srv::SetFilter::Request &req, netft_interfaces::srv::SetFilter::Response &res); 127 | 128 | // Convenience methods 129 | void copyWrench(geometry_msgs::msg::WrenchStamped &in, geometry_msgs::msg::WrenchStamped &out, geometry_msgs::msg::WrenchStamped &bias); 130 | void applyThreshold(double &value, double thresh); 131 | void transformFrame(geometry_msgs::msg::WrenchStamped in_data, geometry_msgs::msg::WrenchStamped &out_data, char target_frame); 132 | void checkMaxForce(); 133 | }; 134 | } 135 | #endif -------------------------------------------------------------------------------- /netft_utils/include/netft_utils/netft_hardware_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // 16 | // Authors: Subhas Das, Denis Stogl 17 | // 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "hardware_interface/handle.hpp" 26 | #include "hardware_interface/hardware_info.hpp" 27 | #include "hardware_interface/sensor_interface.hpp" 28 | #include "hardware_interface/types/hardware_interface_return_values.hpp" 29 | #include "netft_utils/netft_rdt_driver.h" 30 | #include "netft_utils/visibility_control.h" 31 | #include "rclcpp/macros.hpp" 32 | 33 | namespace netft_hardware_interface 34 | { 35 | class NetFTHardwareInterface : public hardware_interface::SensorInterface 36 | { 37 | public: 38 | RCLCPP_SHARED_PTR_DEFINITIONS(NetFTHardwareInterface) 39 | 40 | NETFT_HARDWARE_INTERFACE_PUBLIC 41 | hardware_interface::CallbackReturn on_init( 42 | const hardware_interface::HardwareInfo & info) override; 43 | 44 | NETFT_HARDWARE_INTERFACE_PUBLIC 45 | std::vector export_state_interfaces() override; 46 | 47 | NETFT_HARDWARE_INTERFACE_PUBLIC 48 | hardware_interface::CallbackReturn on_activate( 49 | const rclcpp_lifecycle::State & previous_state) override; 50 | 51 | NETFT_HARDWARE_INTERFACE_PUBLIC 52 | hardware_interface::CallbackReturn on_deactivate( 53 | const rclcpp_lifecycle::State & previous_state) override; 54 | 55 | NETFT_HARDWARE_INTERFACE_PUBLIC 56 | hardware_interface::return_type read( 57 | const rclcpp::Time & time, const rclcpp::Duration & period) override; 58 | 59 | private: 60 | // Store the sensor states 61 | std::vector hw_sensor_states_; 62 | 63 | // For talking to the sensor 64 | std::string ip_address_; 65 | std::unique_ptr ft_driver_; 66 | }; 67 | 68 | } // namespace netft_hardware_interface 69 | -------------------------------------------------------------------------------- /netft_utils/include/netft_utils/netft_rdt_driver.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef NETFT_RDT_DRIVER 36 | #define NETFT_RDT_DRIVER 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | // #include "diagnostic_updater/DiagnosticStatusWrapper.h" 45 | 46 | #include 47 | #include 48 | 49 | namespace netft_rdt_driver 50 | { 51 | 52 | class NetFTRDTDriver 53 | { 54 | public: 55 | // Start receiving data from NetFT device 56 | explicit NetFTRDTDriver(const std::string & address, const std::string & frame_id = "base_link"); 57 | 58 | ~NetFTRDTDriver(); 59 | 60 | //! Get newest RDT data from netFT device 61 | void getData(geometry_msgs::msg::WrenchStamped & data); 62 | 63 | // //! Add device diagnostics status wrapper 64 | // void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d); 65 | 66 | //! Wait for new NetFT data to arrive. 67 | // Returns true if new data has arrived, false it function times out 68 | bool waitForNewData(void); 69 | 70 | protected: 71 | void recvThreadFunc(void); 72 | 73 | //! Asks NetFT to start streaming data. 74 | void startStreaming(void); 75 | 76 | // Gets the calibration info via synchronous TCP request, and sets 77 | // the force_scale_ and torque_scale_ appropriately 78 | bool readCalibrationInformation(const std::string & address); 79 | 80 | enum 81 | { 82 | RDT_PORT = 49152, 83 | TCP_PORT = 49151 84 | }; 85 | std::string address_; 86 | std::string frame_id_; 87 | 88 | boost::asio::io_service io_service_; 89 | boost::asio::ip::udp::socket socket_; 90 | boost::mutex mutex_; 91 | boost::thread recv_thread_; 92 | boost::condition condition_; 93 | volatile bool stop_recv_thread_; 94 | //! True if recv loop is still running 95 | bool recv_thread_running_; 96 | //! Set if recv thread exited because of error 97 | std::string recv_thread_error_msg_; 98 | 99 | //! Newest data received from netft device 100 | geometry_msgs::msg::WrenchStamped new_data_; 101 | //! Count number of received packets 102 | unsigned packet_count_; 103 | //! Count of lost RDT packets using RDT sequence number 104 | unsigned lost_packets_; 105 | //! Counts number of out-of-order (or duplicate) received packets 106 | unsigned out_of_order_count_; 107 | //! Incremental counter for wrench header 108 | unsigned seq_counter_; 109 | 110 | //! Scaling factor for converting raw force values from device into Newtons 111 | double force_scale_; 112 | //! Scaling factor for converting raw torque values into Newton*meters 113 | double torque_scale_; 114 | 115 | //! Packet count last time diagnostics thread published output 116 | unsigned diag_packet_count_; 117 | //! Last time diagnostics was published 118 | rclcpp::Time last_diag_pub_time_; 119 | 120 | //! to keep track of out-of-order or duplicate packet 121 | uint32_t last_rdt_sequence_; 122 | //! to keep track of any error codes reported by netft 123 | uint32_t system_status_; 124 | }; 125 | 126 | } // end namespace netft_rdt_driver 127 | 128 | #endif // NETFT_RDT_DRIVER 129 | -------------------------------------------------------------------------------- /netft_utils/include/netft_utils/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2021 ros2_control Development Team 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /* This header must be included by all rclcpp headers which declare symbols 16 | * which are defined in the rclcpp library. When not building the rclcpp 17 | * library, i.e. when using the headers in other package's code, the contents 18 | * of this header change the visibility of certain symbols which the rclcpp 19 | * library cannot have, but the consuming code must have inorder to link. 20 | */ 21 | 22 | #pragma once 23 | 24 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 25 | // https://gcc.gnu.org/wiki/Visibility 26 | 27 | #if defined _WIN32 || defined __CYGWIN__ 28 | #ifdef __GNUC__ 29 | #define NETFT_HARDWARE_INTERFACE_EXPORT __attribute__((dllexport)) 30 | #define NETFT_HARDWARE_INTERFACE_IMPORT __attribute__((dllimport)) 31 | #else 32 | #define NETFT_HARDWARE_INTERFACE_EXPORT __declspec(dllexport) 33 | #define NETFT_HARDWARE_INTERFACE_IMPORT __declspec(dllimport) 34 | #endif 35 | #ifdef NETFT_HARDWARE_INTERFACE_BUILDING_DLL 36 | #define NETFT_HARDWARE_INTERFACE_PUBLIC NETFT_HARDWARE_INTERFACE_EXPORT 37 | #else 38 | #define NETFT_HARDWARE_INTERFACE_PUBLIC NETFT_HARDWARE_INTERFACE_IMPORT 39 | #endif 40 | #define NETFT_HARDWARE_INTERFACE_PUBLIC_TYPE NETFT_HARDWARE_INTERFACE_PUBLIC 41 | #define NETFT_HARDWARE_INTERFACE_LOCAL 42 | #else 43 | #define NETFT_HARDWARE_INTERFACE_EXPORT __attribute__((visibility("default"))) 44 | #define NETFT_HARDWARE_INTERFACE_IMPORT 45 | #if __GNUC__ >= 4 46 | #define NETFT_HARDWARE_INTERFACE_PUBLIC __attribute__((visibility("default"))) 47 | #define NETFT_HARDWARE_INTERFACE_LOCAL __attribute__((visibility("hidden"))) 48 | #else 49 | #define NETFT_HARDWARE_INTERFACE_PUBLIC 50 | #define NETFT_HARDWARE_INTERFACE_LOCAL 51 | #endif 52 | #define NETFT_HARDWARE_INTERFACE_PUBLIC_TYPE 53 | #endif 54 | -------------------------------------------------------------------------------- /netft_utils/include/netft_utils_lean.h: -------------------------------------------------------------------------------- 1 | #ifndef NET_FT_UTILS_LEAN_H 2 | #define NET_FT_UTILS_LEAN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | /** 19 | * This program takes force/torque data and applies transforms to usable data 20 | */ 21 | namespace netft_utils_lean 22 | { 23 | 24 | class NetftUtilsLean : rclcpp::Node 25 | { 26 | public: 27 | NetftUtilsLean(); 28 | ~NetftUtilsLean(); 29 | 30 | bool initialize(double rate, std::string world, std::string ft, double force = 60.0, double torque = 6.0); 31 | bool setUserInput(std::string world, std::string ft, double force, double torque); 32 | bool run(); 33 | void stop(); 34 | 35 | // Access methods 36 | bool biasSensor(bool toBias); 37 | bool setMax(double fMaxU, double tMaxU, double fMaxB, double tMaxB); 38 | bool setThreshold(double fThresh, double tThresh); 39 | bool setFilter(bool toFilter, double deltaT, double cutoffFreq); 40 | bool isReady(); 41 | bool waitForData(double timeout); 42 | void getWorldData(geometry_msgs::msg::WrenchStamped& data); 43 | void getToolData(geometry_msgs::msg::WrenchStamped& data); 44 | void getRawData(geometry_msgs::msg::WrenchStamped& data); 45 | bool isRunning(); 46 | void setFTAddress(std::string ftAddress); 47 | void setFTTopic(std::string ftTopic); 48 | 49 | private: 50 | 51 | // Initialization 52 | std::string ftAddress; 53 | std::string ftTopic; 54 | bool isInit; 55 | bool hasData; 56 | 57 | //netft class 58 | std::unique_ptr netft; 59 | 60 | // cycle rate 61 | double cycleRate; 62 | 63 | //LPFilter 64 | LPFilter* lp; // Filter 65 | bool isFilterOn; 66 | double deltaTFilter; 67 | double cutoffFrequency; 68 | bool newFilter; 69 | bool lpExists; 70 | 71 | // Transform listener 72 | tf2_ros::TransformListener* listener; 73 | tf2::BufferCore bufferCore; 74 | tf2::Stamped ft_to_world; // Transform from ft frame to world frame 75 | std::string world_frame; 76 | std::string ft_frame; 77 | 78 | // Wrenches used to hold force/torque and bias data 79 | geometry_msgs::msg::WrenchStamped bias; // Wrench containing the current bias data in tool frame 80 | geometry_msgs::msg::WrenchStamped raw_data_tool; // Wrench containing the current raw data from the netft sensor in the tool frame 81 | geometry_msgs::msg::WrenchStamped tf_data_world; // Wrench containing the transformed (world frame) data with bias and threshold applied 82 | geometry_msgs::msg::WrenchStamped tf_data_tool; // Wrench containing the transformed (tool frame) data with bias and threshold applied 83 | geometry_msgs::msg::WrenchStamped zero_wrench; // Wrench of all zeros for convenience 84 | geometry_msgs::msg::WrenchStamped threshold; // Wrench containing thresholds 85 | geometry_msgs::msg::WrenchStamped raw_topic_data; // Wrench containing raw topic data 86 | 87 | bool isBiased; // True if sensor is biased 88 | bool isNewBias; // True if sensor was biased this pass 89 | bool waitingForTransform; // False after initial transform is supplied 90 | bool isActive; // True if run function has been called 91 | 92 | // Variables used to monitor FT violation and send a cancel move message 93 | netft_interfaces::msg::Cancel cancel_msg; 94 | static const int MAX_CANCEL = 5; // Number of times to send cancel message when max force is exceeded 95 | static const int MAX_WAIT = 100; // Number of cycles to wait after cancel message before sending again 96 | int cancel_count; // Counter for times to send cancel message when max force is exceeded 97 | int cancel_wait; // Counter of cycles to wait after cancel message before sending again 98 | double forceMaxB; // Default max force limit to send cancel when FT is biased 99 | double torqueMaxB; // Default max torque limit to send cancel when FT is biased 100 | double forceMaxU; // Default max force limit to send cancel when FT is unbiased 101 | double torqueMaxU; // Default max torque limit to send cancel when FT is unbiased 102 | 103 | // ROS subscribers 104 | rclcpp::Subscription::SharedPtr ft_sub; 105 | // ROS publishers 106 | rclcpp::Publisher::SharedPtr netft_cancel_pub; 107 | rclcpp::Publisher::SharedPtr data_pub; 108 | 109 | // Callback methods 110 | void netftCallback(const geometry_msgs::msg::WrenchStamped& data); 111 | void dataCallback(const geometry_msgs::msg::WrenchStamped::ConstPtr& msg); 112 | 113 | // Threads 114 | std::future monitorThread; 115 | std::future updateThread; 116 | bool toUpdate; 117 | bool toMonitor; 118 | 119 | bool update(); 120 | 121 | // Convenience methods 122 | void copyWrench(geometry_msgs::msg::WrenchStamped &in, geometry_msgs::msg::WrenchStamped &out, geometry_msgs::msg::WrenchStamped &bias); 123 | void applyThreshold(double &value, double thresh); 124 | void transformFrame(geometry_msgs::msg::WrenchStamped in_data, geometry_msgs::msg::WrenchStamped &out_data, char target_frame); 125 | void checkMaxForce(); 126 | bool monitorData(); 127 | 128 | static const bool DEBUG_DATA = true; 129 | }; 130 | 131 | } 132 | #endif 133 | -------------------------------------------------------------------------------- /netft_utils/launch/netft.launch.py: -------------------------------------------------------------------------------- 1 | from launch_ros.actions import Node 2 | from launch_ros.parameter_descriptions import ParameterFile, ParameterValue 3 | from launch_ros.substitutions import FindPackageShare 4 | 5 | from launch import LaunchDescription 6 | from launch.actions import DeclareLaunchArgument, OpaqueFunction 7 | from launch.conditions import IfCondition, UnlessCondition 8 | from launch.substitutions import ( 9 | Command, 10 | FindExecutable, 11 | LaunchConfiguration, 12 | PathJoinSubstitution, 13 | ) 14 | 15 | 16 | def launch_setup(context, *args, **kwargs): 17 | # Initialize Arguments 18 | node_namespace = LaunchConfiguration("node_namespace") 19 | 20 | robot_description_content = Command( 21 | [ 22 | PathJoinSubstitution([FindExecutable(name="xacro")]), 23 | " ", 24 | PathJoinSubstitution( 25 | [FindPackageShare("netft_utils"), "urdf", "example_description.urdf.xacro"] 26 | ), 27 | ] 28 | ) 29 | robot_description = { 30 | "robot_description": ParameterValue(robot_description_content, value_type=str) 31 | } 32 | 33 | control_configs = PathJoinSubstitution( 34 | [FindPackageShare("netft_utils"), "config", "control_params.yaml"] 35 | ) 36 | 37 | control_node = Node( 38 | package="controller_manager", 39 | executable="ros2_control_node", 40 | namespace=node_namespace, 41 | parameters=[ 42 | robot_description, 43 | ParameterFile(control_configs, allow_substs=True), 44 | ], 45 | output="screen", 46 | ) 47 | 48 | ft_broadcaster = Node( 49 | package="controller_manager", 50 | executable="spawner", 51 | namespace=node_namespace, 52 | arguments=[ 53 | "force_torque_sensor_broadcaster", 54 | "--controller-manager", 55 | "controller_manager", 56 | "-n", 57 | node_namespace, 58 | "--controller-manager-timeout", 59 | "10", 60 | ], 61 | output="screen", 62 | ) 63 | 64 | nodes_to_start = [ 65 | control_node, 66 | ft_broadcaster] 67 | 68 | return nodes_to_start 69 | 70 | 71 | def generate_launch_description(): 72 | declared_arguments = [] 73 | declared_arguments.append( 74 | DeclareLaunchArgument( 75 | "node_namespace", 76 | default_value="/", 77 | description="The namespace to put all the nodes into", 78 | ) 79 | ) 80 | 81 | return LaunchDescription( 82 | declared_arguments + [OpaqueFunction(function=launch_setup)] 83 | ) 84 | -------------------------------------------------------------------------------- /netft_utils/netft_hardware_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /netft_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | netft_utils 4 | 1.0.1 5 | C++ class and ROS node for ATI force/torque sensors connected to a Netbox. Includes gravity compensation and transformations. 6 | 7 | Alex von Sternberg 8 | Derek King 9 | Andy Zelenak 10 | 11 | Andy Zelenak 12 | 13 | http://wiki.ros.org/netft_utils 14 | 15 | See License.txt 16 | 17 | 18 | rclcpp 19 | rclcpp_lifecycle 20 | hardware_interface 21 | pluginlib 22 | netft_interfaces 23 | geometry_msgs 24 | diagnostic_updater 25 | tf2 26 | rosidl_default_generators 27 | 28 | rosidl_interface_packages 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /netft_utils/src/lpfilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2016, Los Alamos National Security, LLC 3 | All rights reserved. 4 | Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL. 5 | 6 | Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | 13 | Author: Alex von Sternberg 14 | */ 15 | 16 | #include 17 | 18 | LPFilter::LPFilter(double deltaT, double cutoffFrequency, int numElements): 19 | initialized(false), 20 | noElements(0), 21 | omega_a(0.0), 22 | a0(0.0), 23 | a1(0.0), 24 | a2(0.0), 25 | b1(0.0), 26 | b2(0.0) 27 | { 28 | initialized = true; 29 | if(numElements<=0) 30 | { 31 | RCLCPP_ERROR_STREAM("LPFilter was passed invalid number of elements. Not filtering."); 32 | initialized = false; 33 | } 34 | else 35 | { 36 | noElements = numElements; 37 | } 38 | if(deltaT<=0) 39 | { 40 | RCLCPP_ERROR_STREAM("LPFilter was passed invalid deltaT. Not Filtering."); 41 | initialized = false; 42 | } 43 | if(cutoffFrequency <=0) 44 | { 45 | RCLCPP_ERROR_STREAM("LPFilter was passed invalid cuttoffFrequency. Not Filtering."); 46 | initialized = false; 47 | } 48 | else 49 | { 50 | cutoffFrequency *= (2*M_PI); 51 | omega_a = tan(cutoffFrequency*deltaT/2.0); 52 | double den = omega_a*omega_a+sqrt(2.0*omega_a+1.0); 53 | a0 = (omega_a*omega_a)/den; 54 | a1 = (2.0*omega_a*omega_a)/den; 55 | a2 = a0; 56 | b1 = 2.0*(omega_a*omega_a-1.0)/den; 57 | b2 = (omega_a*omega_a-sqrt(2.0)*omega_a+1)/den; 58 | in1.resize(noElements); 59 | in2.resize(noElements); 60 | out1.resize(noElements); 61 | out2.resize(noElements); 62 | RCLCPP_INFO_STREAM("cutoffFrequency: " << cutoffFrequency << ". omega_a: " << omega_a << ". den: " << den << ". a0: " << a0 << ". a1: " << a1 << ". a2: " << a2 << ". b1: " << b1 << ". b2: " << b2); 63 | } 64 | } 65 | 66 | bool LPFilter::update(std::vector input, std::vector& output) 67 | { 68 | if(!initialized) 69 | { 70 | RCLCPP_ERROR_STREAM("LPFilter was not initialized correctly. Not filtering data."); 71 | return false; 72 | } 73 | if(input.size() != in1.size() || output.size() != out1.size()) 74 | { 75 | RCLCPP_ERROR_STREAM("LPFilter incorrect input or output size"); 76 | return false; 77 | } 78 | for(int i=0; i 22 | #include 23 | #include 24 | #include 25 | 26 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 27 | #include "rclcpp/rclcpp.hpp" 28 | 29 | namespace netft_hardware_interface 30 | { 31 | hardware_interface::CallbackReturn NetFTHardwareInterface::on_init( 32 | const hardware_interface::HardwareInfo & info) 33 | { 34 | if ( 35 | hardware_interface::SensorInterface::on_init(info) != 36 | hardware_interface::CallbackReturn::SUCCESS) { 37 | return hardware_interface::CallbackReturn::ERROR; 38 | } 39 | 40 | ip_address_ = info_.hardware_parameters["address"]; 41 | 42 | hw_sensor_states_.resize( 43 | info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); 44 | 45 | return hardware_interface::CallbackReturn::SUCCESS; 46 | } 47 | 48 | std::vector NetFTHardwareInterface::export_state_interfaces() 49 | { 50 | std::vector state_interfaces; 51 | 52 | // export sensor state interface 53 | for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) { 54 | state_interfaces.emplace_back(hardware_interface::StateInterface( 55 | info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); 56 | } 57 | 58 | return state_interfaces; 59 | } 60 | 61 | hardware_interface::CallbackReturn NetFTHardwareInterface::on_activate( 62 | const rclcpp_lifecycle::State & /*previous_state*/) 63 | { 64 | RCLCPP_INFO_STREAM( 65 | rclcpp::get_logger("NetFTHardwareInterface"), "Opening sensor at: " << ip_address_); 66 | 67 | try { 68 | ft_driver_ = std::make_unique(ip_address_); 69 | } catch (const std::runtime_error& e) { 70 | RCLCPP_ERROR_STREAM( 71 | rclcpp::get_logger("NetFTHardwareInterface"), "Failed to reach sensor at: " << ip_address_); 72 | throw e; 73 | } 74 | 75 | return hardware_interface::CallbackReturn::SUCCESS; 76 | } 77 | 78 | hardware_interface::CallbackReturn NetFTHardwareInterface::on_deactivate( 79 | const rclcpp_lifecycle::State & /*previous_state*/) 80 | { 81 | return hardware_interface::CallbackReturn::SUCCESS; 82 | } 83 | 84 | hardware_interface::return_type NetFTHardwareInterface::read( 85 | const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) 86 | { 87 | geometry_msgs::msg::WrenchStamped wrench; 88 | if (ft_driver_->waitForNewData()) { 89 | ft_driver_->getData(wrench); 90 | } 91 | 92 | hw_sensor_states_.at(0) = wrench.wrench.force.x; 93 | hw_sensor_states_.at(1) = wrench.wrench.force.y; 94 | hw_sensor_states_.at(2) = wrench.wrench.force.z; 95 | hw_sensor_states_.at(3) = wrench.wrench.torque.x; 96 | hw_sensor_states_.at(4) = wrench.wrench.torque.y; 97 | hw_sensor_states_.at(5) = wrench.wrench.torque.z; 98 | 99 | return hardware_interface::return_type::OK; 100 | } 101 | 102 | } // namespace netft_hardware_interface 103 | 104 | #include "pluginlib/class_list_macros.hpp" 105 | 106 | PLUGINLIB_EXPORT_CLASS( 107 | netft_hardware_interface::NetFTHardwareInterface, hardware_interface::SensorInterface) 108 | -------------------------------------------------------------------------------- /netft_utils/src/netft_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /** 36 | * Simple stand-alone ROS node that takes data from NetFT sensor and 37 | * Publishes it ROS topic 38 | */ 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | // #include 46 | // #include "diagnostic_updater/DiagnosticStatusWrapper.h" 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | namespace po = boost::program_options; 55 | using namespace std; 56 | 57 | int main(int argc, char ** argv) 58 | { 59 | rclcpp::init(argc, argv); 60 | 61 | float pub_rate_hz; 62 | string address; 63 | string frame_id; 64 | 65 | po::options_description desc("Options"); 66 | desc.add_options()("--ros-args", "ros arguments")("help", "display help")( 67 | "rate", po::value(&pub_rate_hz)->default_value(500.0), "set publish rate (in hertz)")( 68 | "address", po::value(&address), "IP address of NetFT box")( 69 | "frame_id", po::value(&frame_id)->default_value("base_link"), 70 | "frame_id for Wrench msgs"); 71 | 72 | po::positional_options_description p; 73 | p.add("address", 1); 74 | p.add("frame_id", 1); 75 | 76 | po::variables_map vm; 77 | po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); 78 | po::notify(vm); 79 | 80 | if (vm.count("help")) { 81 | cout << desc << endl; 82 | //usage(progname); 83 | exit(EXIT_SUCCESS); 84 | } 85 | 86 | if (!vm.count("address")) { 87 | cout << desc << endl; 88 | cerr << "Please specify address of NetFT" << endl; 89 | exit(EXIT_FAILURE); 90 | } 91 | 92 | std_msgs::msg::Bool is_ready; 93 | std::shared_ptr netft; 94 | 95 | // Set up ROS publishers 96 | auto node = std::make_shared("netft_node"); 97 | const rclcpp::QoS qos(10); 98 | rclcpp::Publisher::SharedPtr ready_pub = node->create_publisher("netft_ready", qos); 99 | rclcpp::Publisher::SharedPtr geo_pub = node->create_publisher("netft_data", 100); 100 | 101 | try { 102 | netft = std::make_shared(address, frame_id); 103 | is_ready.data = true; 104 | ready_pub->publish(is_ready); 105 | } catch (std::runtime_error & e) { 106 | is_ready.data = false; 107 | ready_pub->publish(is_ready); 108 | RCLCPP_ERROR_STREAM(node->get_logger(), "Error opening NetFT: " << e.what()); 109 | } 110 | 111 | rclcpp::Rate pub_rate(pub_rate_hz); 112 | geometry_msgs::msg::WrenchStamped data; 113 | 114 | // rclcpp::Duration diag_pub_duration(std::chrono::milliseconds(1000)); 115 | // rclcpp::Publisher diag_pub = netft->create_publisher("/diagnostics", 2); 116 | // diagnostic_msgs::DiagnosticArray diag_array; 117 | // diag_array.status.reserve(1); 118 | // diagnostic_updater::DiagnosticStatusWrapper diag_status; 119 | // rclcpp::Time last_diag_pub_time(rclcpp::Clock().now()); 120 | 121 | while (rclcpp::ok()) { 122 | // Publish netft data when data is recieved. 123 | if (netft->waitForNewData()) { 124 | netft->getData(data); 125 | geo_pub->publish(data); 126 | } 127 | 128 | // TODO: removed diagnostics for now will implement in the future. 129 | // rclcpp::Time current_time(rclcpp::Clock().now()); 130 | // if ( (current_time - last_diag_pub_time) > diag_pub_duration ) 131 | // { 132 | // diag_array.status.clear(); 133 | // netft->diagnostics(diag_status); 134 | // diag_array.status.push_back(diag_status); 135 | // diag_array.header.stamp = rclcpp::Clock().now(); 136 | // diag_pub.publish(diag_array); 137 | // ready_pub->publish(is_ready); 138 | // last_diag_pub_time = current_time; 139 | // } 140 | 141 | rclcpp::spin_some(node); 142 | pub_rate.sleep(); 143 | } 144 | 145 | return 0; 146 | } 147 | -------------------------------------------------------------------------------- /netft_utils/src/netft_rdt_driver.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "netft_utils/netft_rdt_driver.h" 36 | 37 | #include 38 | #include 39 | 40 | using boost::asio::ip::udp; 41 | 42 | namespace netft_rdt_driver 43 | { 44 | struct RDTRecord 45 | { 46 | uint32_t rdt_sequence_; 47 | uint32_t ft_sequence_; 48 | uint32_t status_; 49 | int32_t fx_; 50 | int32_t fy_; 51 | int32_t fz_; 52 | int32_t tx_; 53 | int32_t ty_; 54 | int32_t tz_; 55 | 56 | enum 57 | { 58 | RDT_RECORD_SIZE = 36 59 | }; 60 | void unpack(const uint8_t * buffer); 61 | static uint32_t unpack32(const uint8_t * buffer); 62 | }; 63 | 64 | uint32_t RDTRecord::unpack32(const uint8_t * buffer) 65 | { 66 | return (static_cast(buffer[0]) << 24) | (static_cast(buffer[1]) << 16) | 67 | (static_cast(buffer[2]) << 8) | (static_cast(buffer[3]) << 0); 68 | } 69 | 70 | void RDTRecord::unpack(const uint8_t * buffer) 71 | { 72 | rdt_sequence_ = unpack32(buffer + 0); 73 | ft_sequence_ = unpack32(buffer + 4); 74 | status_ = unpack32(buffer + 8); 75 | fx_ = static_cast(unpack32(buffer + 12)); 76 | fy_ = static_cast(unpack32(buffer + 16)); 77 | fz_ = static_cast(unpack32(buffer + 20)); 78 | tx_ = static_cast(unpack32(buffer + 24)); 79 | ty_ = static_cast(unpack32(buffer + 28)); 80 | tz_ = static_cast(unpack32(buffer + 32)); 81 | } 82 | 83 | struct RDTCommand 84 | { 85 | uint16_t command_header_{}; 86 | uint16_t command_{}; 87 | uint32_t sample_count_{}; 88 | 89 | RDTCommand() : command_header_(HEADER) 90 | { 91 | // empty 92 | } 93 | 94 | enum 95 | { 96 | HEADER = 0x1234 97 | }; 98 | 99 | // Possible values for command_ 100 | enum 101 | { 102 | CMD_STOP_STREAMING = 0, 103 | CMD_START_HIGH_SPEED_STREAMING = 2, 104 | // More command values are available but are not used by this driver 105 | }; 106 | 107 | // Special values for sample count 108 | enum 109 | { 110 | INFINITE_SAMPLES = 0 111 | }; 112 | 113 | enum 114 | { 115 | RDT_COMMAND_SIZE = 8 116 | }; 117 | 118 | //!Packet structure into buffer for network transport 119 | // Buffer should be RDT_COMMAND_SIZE 120 | void pack(uint8_t * buffer) const; 121 | }; 122 | 123 | void RDTCommand::pack(uint8_t * buffer) const 124 | { 125 | // Data is big-endian 126 | buffer[0] = (command_header_ >> 8) & 0xFF; 127 | buffer[1] = (command_header_ >> 0) & 0xFF; 128 | buffer[2] = (command_ >> 8) & 0xFF; 129 | buffer[3] = (command_ >> 0) & 0xFF; 130 | buffer[4] = (sample_count_ >> 8) & 0xFF; 131 | buffer[5] = (sample_count_ >> 0) & 0xFF; 132 | buffer[6] = (sample_count_ >> 8) & 0xFF; 133 | buffer[7] = (sample_count_ >> 0) & 0xFF; 134 | } 135 | 136 | struct CalibrationInfoResponse 137 | { 138 | uint16_t header_; 139 | uint8_t force_units_; 140 | uint8_t torque_units_; 141 | uint32_t counts_per_force_; 142 | uint32_t counts_per_torque_; 143 | uint16_t scale_factors_[6]; 144 | 145 | // Possible values for force units 146 | enum 147 | { 148 | FORCE_UNIT_POUND = 1, 149 | FORCE_UNIT_NEWTON = 2, 150 | FORCE_UNIT_KILOPOUND = 3, 151 | FORCE_UNIT_KILONEWTON = 4, 152 | FORCE_UNIT_KILOGRAM = 5, 153 | FORCE_UNIT_GRAM = 6, 154 | }; 155 | 156 | // Possible values for torque units 157 | enum 158 | { 159 | TORQUE_UNIT_POUND_INCH = 1, 160 | TORQUE_UNIT_POUND_FOOT = 2, 161 | TORQUE_UNIT_NEWTON_METER = 3, 162 | TORQUE_UNIT_NEWTON_MILLIMETER = 4, 163 | TORQUE_UNIT_KILOGRAM_CENTIMETER = 5, 164 | TORQUE_UNIT_KILONEWTON_METER = 6, 165 | }; 166 | 167 | enum 168 | { 169 | CALIB_INFO_RESPONSE_SIZE = 24 170 | }; 171 | void unpack(const uint8_t * buffer); 172 | static uint32_t unpack32(const uint8_t * buffer); 173 | static uint16_t unpack16(const uint8_t * buffer); 174 | static uint8_t unpack8(const uint8_t * buffer); 175 | std::string getForceUnitString(); 176 | std::string getTorqueUnitString(); 177 | }; 178 | 179 | uint32_t CalibrationInfoResponse::unpack32(const uint8_t * buffer) 180 | { 181 | return (static_cast(buffer[0]) << 24) | (static_cast(buffer[1]) << 16) | 182 | (static_cast(buffer[2]) << 8) | (static_cast(buffer[3]) << 0); 183 | } 184 | 185 | uint16_t CalibrationInfoResponse::unpack16(const uint8_t * buffer) 186 | { 187 | return (static_cast(buffer[0]) << 8) | (static_cast(buffer[1]) << 0); 188 | } 189 | 190 | uint8_t CalibrationInfoResponse::unpack8(const uint8_t * buffer) 191 | { 192 | return (static_cast(buffer[0]) << 0); 193 | } 194 | 195 | void CalibrationInfoResponse::unpack(const uint8_t * buffer) 196 | { 197 | header_ = unpack16(buffer + 0); 198 | force_units_ = unpack8(buffer + 2); 199 | torque_units_ = unpack8(buffer + 3); 200 | counts_per_force_ = unpack32(buffer + 4); 201 | counts_per_torque_ = unpack32(buffer + 8); 202 | scale_factors_[0] = unpack16(buffer + 12); 203 | scale_factors_[1] = unpack16(buffer + 14); 204 | scale_factors_[2] = unpack16(buffer + 16); 205 | scale_factors_[3] = unpack16(buffer + 18); 206 | scale_factors_[4] = unpack16(buffer + 20); 207 | scale_factors_[5] = unpack16(buffer + 22); 208 | } 209 | 210 | std::string CalibrationInfoResponse::getForceUnitString() 211 | { 212 | switch (force_units_) { 213 | case FORCE_UNIT_POUND: 214 | return "Pounds"; 215 | case FORCE_UNIT_NEWTON: 216 | return "Newtons"; 217 | case FORCE_UNIT_KILOPOUND: 218 | return "Kilopounds"; 219 | case FORCE_UNIT_KILONEWTON: 220 | return "Kilonewtons"; 221 | case FORCE_UNIT_KILOGRAM: 222 | return "Kilograms"; 223 | case FORCE_UNIT_GRAM: 224 | return "Grams"; 225 | default: 226 | return "Unknown units"; 227 | } 228 | } 229 | 230 | std::string CalibrationInfoResponse::getTorqueUnitString() 231 | { 232 | switch (torque_units_) { 233 | case TORQUE_UNIT_POUND_INCH: 234 | return "Inch-Pounds"; 235 | case TORQUE_UNIT_POUND_FOOT: 236 | return "Pound-Feet"; 237 | case TORQUE_UNIT_NEWTON_METER: 238 | return "Newton-Meters"; 239 | case TORQUE_UNIT_NEWTON_MILLIMETER: 240 | return "Newton-Millimeters"; 241 | case TORQUE_UNIT_KILOGRAM_CENTIMETER: 242 | return "Kilogram-Centimeters"; 243 | case TORQUE_UNIT_KILONEWTON_METER: 244 | return "Kilonewton-Meters"; 245 | default: 246 | return "Unknown units"; 247 | } 248 | } 249 | 250 | struct CalibrationInfoCommand 251 | { 252 | uint8_t command_{}; 253 | uint8_t reserved_[19] = {0}; 254 | 255 | CalibrationInfoCommand() : command_(READCALINFO) {} 256 | 257 | // Possible values for command_ 258 | enum 259 | { 260 | READFT = 0, 261 | READCALINFO = 1, 262 | WRITETRANSFORM = 2, 263 | WRITECONDITION = 3, 264 | }; 265 | 266 | enum 267 | { 268 | CALIB_INFO_COMMAND_SIZE = 20 269 | }; 270 | 271 | //!Packet structure into buffer for network transport 272 | // Buffer should be CALIB_INFO_COMMAND_SIZE 273 | void pack(uint8_t * buffer) const; 274 | }; 275 | 276 | void CalibrationInfoCommand::pack(uint8_t * buffer) const 277 | { 278 | buffer[0] = command_; 279 | for (size_t i = 1; i < CALIB_INFO_COMMAND_SIZE; ++i) { 280 | buffer[i] = 0x0; 281 | } 282 | } 283 | 284 | NetFTRDTDriver::NetFTRDTDriver(const std::string & address, const std::string & frame_id) 285 | : address_(address), 286 | frame_id_(frame_id), 287 | socket_(io_service_), 288 | stop_recv_thread_(false), 289 | recv_thread_running_(false), 290 | packet_count_(0), 291 | lost_packets_(0), 292 | out_of_order_count_(0), 293 | seq_counter_(0), 294 | diag_packet_count_(0), 295 | last_diag_pub_time_(rclcpp::Clock().now()), 296 | last_rdt_sequence_(0), 297 | system_status_(0) 298 | { 299 | // Construct UDP socket 300 | const udp::endpoint netft_endpoint(boost::asio::ip::address_v4::from_string(address), RDT_PORT); 301 | socket_.open(udp::v4()); 302 | socket_.connect(netft_endpoint); 303 | 304 | // Read the calibration information 305 | if (!readCalibrationInformation(address)) { 306 | throw std::runtime_error("TCP Request for calibration information failed"); 307 | } 308 | 309 | // Start receive thread 310 | recv_thread_ = boost::thread(&NetFTRDTDriver::recvThreadFunc, this); 311 | 312 | // Since start steaming command is sent with UDP packet, 313 | // the packet could be lost, retry startup 10 times before giving up 314 | for (int i = 0; i < 10; ++i) { 315 | startStreaming(); 316 | if (waitForNewData()) break; 317 | } 318 | { 319 | boost::unique_lock lock(mutex_); 320 | if (packet_count_ == 0) { 321 | throw std::runtime_error("No data received from NetFT device"); 322 | } 323 | } 324 | } 325 | 326 | NetFTRDTDriver::~NetFTRDTDriver() 327 | { 328 | // TODO stop transmission, 329 | // stop thread 330 | stop_recv_thread_ = true; 331 | if (!recv_thread_.timed_join(boost::posix_time::time_duration(0, 0, 1, 0))) { 332 | recv_thread_.interrupt(); 333 | } 334 | socket_.close(); 335 | } 336 | 337 | bool NetFTRDTDriver::waitForNewData() 338 | { 339 | // Wait upto 100ms for new data 340 | bool got_new_data; 341 | { 342 | boost::mutex::scoped_lock lock(mutex_); 343 | const unsigned current_packet_count = packet_count_; 344 | condition_.timed_wait(lock, boost::posix_time::milliseconds(100)); 345 | got_new_data = packet_count_ != current_packet_count; 346 | } 347 | return got_new_data; 348 | } 349 | 350 | bool NetFTRDTDriver::readCalibrationInformation(const std::string & address) 351 | { 352 | // Connect to socket 353 | using boost::asio::ip::tcp; 354 | tcp::socket calibration_socket(io_service_); 355 | calibration_socket.connect( 356 | tcp::endpoint(boost::asio::ip::address::from_string(address), TCP_PORT)); 357 | 358 | // Set up request 359 | CalibrationInfoCommand info_request; 360 | info_request.command_ = CalibrationInfoCommand::READCALINFO; 361 | uint8_t buffer[CalibrationInfoCommand::CALIB_INFO_COMMAND_SIZE]; 362 | info_request.pack(buffer); 363 | 364 | // Send request 365 | boost::system::error_code error; 366 | boost::asio::write(calibration_socket, boost::asio::buffer(buffer), error); 367 | if (error) { 368 | // RCLCPP_ERROR_STREAM( 369 | // get_logger(), "Failed to send calibration info request: " << error.message()); 370 | return false; 371 | } 372 | 373 | uint8_t read_buffer[CalibrationInfoResponse::CALIB_INFO_RESPONSE_SIZE + 1]; 374 | const size_t len = calibration_socket.read_some( 375 | boost::asio::buffer(read_buffer, CalibrationInfoResponse::CALIB_INFO_RESPONSE_SIZE + 1)); 376 | if (len != CalibrationInfoResponse::CALIB_INFO_RESPONSE_SIZE) { 377 | // RCLCPP_ERROR_STREAM( 378 | // get_logger(), 379 | // "Calibration response was length: " << len << ", but expected " 380 | // << CalibrationInfoResponse::CALIB_INFO_RESPONSE_SIZE); 381 | return false; 382 | } 383 | 384 | // Unpack response 385 | CalibrationInfoResponse calibration_info; 386 | calibration_info.unpack(read_buffer); 387 | 388 | // Check the info and use it 389 | force_scale_ = 1.0 / calibration_info.counts_per_force_; 390 | torque_scale_ = 1.0 / calibration_info.counts_per_torque_; 391 | 392 | if (calibration_info.force_units_ == CalibrationInfoResponse::FORCE_UNIT_NEWTON) { 393 | // RCLCPP_INFO_STREAM( 394 | // get_logger(), 395 | // "Read " << calibration_info.counts_per_force_ << " counts per force, with units Newtons"); 396 | } else { 397 | // RCLCPP_WARN_STREAM( 398 | // get_logger(), "Expected calibration to have units Newtons, got: " 399 | // << calibration_info.getForceUnitString() << ". Publishing non-SI units"); 400 | } 401 | 402 | if (calibration_info.torque_units_ == CalibrationInfoResponse::TORQUE_UNIT_NEWTON_METER) { 403 | // RCLCPP_INFO_STREAM( 404 | // get_logger(), "Read " << calibration_info.counts_per_torque_ 405 | // << " counts per torque, with units Newton-Meters"); 406 | } else { 407 | // RCLCPP_WARN_STREAM( 408 | // get_logger(), "Expected calibration to have units Newton-Meters, got: " 409 | // << calibration_info.getTorqueUnitString() << ". Publishing non-SI units"); 410 | } 411 | 412 | return true; 413 | } 414 | 415 | void NetFTRDTDriver::startStreaming(void) 416 | { 417 | // Command NetFT to start data transmission 418 | RDTCommand start_transmission; 419 | start_transmission.command_ = RDTCommand::CMD_START_HIGH_SPEED_STREAMING; 420 | start_transmission.sample_count_ = RDTCommand::INFINITE_SAMPLES; 421 | // TODO change buffer into boost::array 422 | uint8_t buffer[RDTCommand::RDT_COMMAND_SIZE]; 423 | start_transmission.pack(buffer); 424 | socket_.send(boost::asio::buffer(buffer, RDTCommand::RDT_COMMAND_SIZE)); 425 | } 426 | 427 | void NetFTRDTDriver::recvThreadFunc() 428 | { 429 | try { 430 | recv_thread_running_ = true; 431 | RDTRecord rdt_record{}; 432 | geometry_msgs::msg::WrenchStamped tmp_data; 433 | uint8_t buffer[RDTRecord::RDT_RECORD_SIZE + 1]; 434 | while (!stop_recv_thread_) { 435 | size_t len = socket_.receive(boost::asio::buffer(buffer, RDTRecord::RDT_RECORD_SIZE + 1)); 436 | if (len != RDTRecord::RDT_RECORD_SIZE) { 437 | // RCLCPP_WARN( 438 | // get_logger(), "Receive size of %d bytes does not match expected size of %d", 439 | // static_cast(len), static_cast(RDTRecord::RDT_RECORD_SIZE)); 440 | } else { 441 | rdt_record.unpack(buffer); 442 | if (rdt_record.status_ != 0) { 443 | // Latch any system status error code 444 | boost::unique_lock lock(mutex_); 445 | system_status_ = rdt_record.status_; 446 | } 447 | const auto seqdiff = static_cast(rdt_record.rdt_sequence_ - last_rdt_sequence_); 448 | last_rdt_sequence_ = rdt_record.rdt_sequence_; 449 | if (seqdiff < 1) { 450 | boost::unique_lock lock(mutex_); 451 | // Don't use data that is old 452 | ++out_of_order_count_; 453 | } else { 454 | tmp_data.header.stamp = rclcpp::Clock().now(); 455 | tmp_data.header.frame_id = frame_id_; 456 | tmp_data.wrench.force.x = static_cast(rdt_record.fx_) * force_scale_; 457 | tmp_data.wrench.force.y = static_cast(rdt_record.fy_) * force_scale_; 458 | tmp_data.wrench.force.z = static_cast(rdt_record.fz_) * force_scale_; 459 | tmp_data.wrench.torque.x = static_cast(rdt_record.tx_) * torque_scale_; 460 | tmp_data.wrench.torque.y = static_cast(rdt_record.ty_) * torque_scale_; 461 | tmp_data.wrench.torque.z = static_cast(rdt_record.tz_) * torque_scale_; 462 | { 463 | boost::unique_lock lock(mutex_); 464 | new_data_ = tmp_data; 465 | lost_packets_ += (seqdiff - 1); 466 | ++packet_count_; 467 | condition_.notify_all(); 468 | } 469 | } 470 | } 471 | } // end while 472 | } catch (std::exception & e) { 473 | recv_thread_running_ = false; 474 | { 475 | boost::unique_lock lock(mutex_); 476 | recv_thread_error_msg_ = e.what(); 477 | } 478 | } 479 | } 480 | 481 | void NetFTRDTDriver::getData(geometry_msgs::msg::WrenchStamped & data) 482 | { 483 | { 484 | boost::unique_lock lock(mutex_); 485 | data = new_data_; 486 | } 487 | } 488 | 489 | // TODO: diagnostics not implemented for ros2 yet 490 | // void NetFTRDTDriver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d) 491 | // { 492 | // // Publish diagnostics 493 | // d.name = "NetFT RDT Driver : " + address_; 494 | // 495 | // d.summary(d.OK, "OK"); 496 | // d.hardware_id = "0"; 497 | // 498 | // if (diag_packet_count_ == packet_count_) 499 | // { 500 | // d.mergeSummary(d.ERROR, "No new data in last second"); 501 | // } 502 | // 503 | // if (!recv_thread_running_) 504 | // { 505 | // d.mergeSummaryf(d.ERROR, "Receive thread has stopped : %s", recv_thread_error_msg_.c_str()); 506 | // } 507 | // 508 | // if (system_status_ != 0) 509 | // { 510 | // d.mergeSummaryf(d.ERROR, "NetFT reports error 0x%08x", system_status_); 511 | // } 512 | // 513 | // rclcpp::Clock clock; 514 | // rclcpp::Time current_time = clock.now(); 515 | // double recv_rate = double(int32_t(packet_count_ - diag_packet_count_)) / (current_time - last_diag_pub_time_).seconds(); 516 | // 517 | // d.clear(); 518 | // d.addf("IP Address", "%s", address_.c_str()); 519 | // d.addf("System status", "0x%08x", system_status_); 520 | // d.addf("Good packets", "%u", packet_count_); 521 | // d.addf("Lost packets", "%u", lost_packets_); 522 | // d.addf("Out-of-order packets", "%u", out_of_order_count_); 523 | // d.addf("Recv rate (pkt/sec)", "%.2f", recv_rate); 524 | // d.addf("Force scale (N/bit)", "%f", force_scale_); 525 | // d.addf("Torque scale (Nm/bit)", "%f", torque_scale_); 526 | // 527 | // geometry_msgs::msg::WrenchStamped data; 528 | // getData(data); 529 | // d.addf("Force X (N)", "%f", data.wrench.force.x); 530 | // d.addf("Force Y (N)", "%f", data.wrench.force.y); 531 | // d.addf("Force Z (N)", "%f", data.wrench.force.z); 532 | // d.addf("Torque X (Nm)", "%f", data.wrench.torque.x); 533 | // d.addf("Torque Y (Nm)", "%f", data.wrench.torque.y); 534 | // d.addf("Torque Z (Nm)", "%f", data.wrench.torque.z); 535 | // 536 | // last_diag_pub_time_ = current_time; 537 | // diag_packet_count_ = packet_count_; 538 | // } 539 | } // end namespace netft_rdt_driver 540 | -------------------------------------------------------------------------------- /netft_utils/src/netft_utils.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | Copyright (c) 2016, Los Alamos National Security, LLC 4 | All rights reserved. 5 | Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL. 6 | 7 | Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | 12 | THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | 14 | Authors: Alex von Sternberg, Andy Zelenak 15 | */ 16 | 17 | #include 18 | namespace netft_utils 19 | { 20 | NetftUtils::NetftUtils() : Node("netft_utils"), 21 | isFilterOn(false), 22 | deltaTFilter(0.0), 23 | cutoffFrequency(0.0), 24 | newFilter(false), 25 | isBiased(false), 26 | isGravityBiased(false), 27 | isNewBias(false), 28 | isNewGravityBias(false), 29 | cancel_count(MAX_CANCEL), 30 | cancel_wait(MAX_WAIT), 31 | forceMaxB(10.0), 32 | torqueMaxB(0.8), 33 | forceMaxU(50.0), 34 | torqueMaxU(5.0), 35 | payloadWeight(0.), 36 | payloadLeverArm(0.) 37 | { 38 | 39 | } 40 | 41 | NetftUtils::~NetftUtils() 42 | { 43 | delete listener; 44 | delete lp; 45 | } 46 | 47 | rclcpp::Logger NetftUtils::getLog() 48 | { 49 | return this->get_logger(); 50 | } 51 | 52 | void NetftUtils::initialize() 53 | { 54 | //lp = new LPFilter(0.002,200,6); 55 | 56 | //Zero out the zero wrench 57 | zero_wrench.wrench.force.x = 0.0; 58 | zero_wrench.wrench.force.y = 0.0; 59 | zero_wrench.wrench.force.z = 0.0; 60 | zero_wrench.wrench.torque.x = 0.0; 61 | zero_wrench.wrench.torque.y = 0.0; 62 | zero_wrench.wrench.torque.z = 0.0; 63 | 64 | //Initialize cancel message 65 | cancel_msg.cancel = false; 66 | 67 | //Listen to the transfomation from the ft sensor to world frame. 68 | listener = new tf2_ros::TransformListener(bufferCore); 69 | 70 | //Subscribe to the NetFT topic. 71 | raw_data_sub = create_subscription("netft_data",100, &NetftUtils::netftCallback); 72 | 73 | //Publish on the /netft_transformed_data topic. Queue up to 100000 data points 74 | netft_raw_world_data_pub = create_publisher("raw_world", 100000); 75 | netft_world_data_pub = create_publisher("transformed_world", 100000); 76 | netft_tool_data_pub = create_publisher("transformed_tool", 100000); 77 | netft_cancel_pub = create_publisher("cancel", 100000); 78 | 79 | //Advertise bias and threshold services 80 | bias_service = create_service("bias", &NetftUtils::fixedOrientationBias); 81 | gravity_comp_service = create_service("gravity_comp", &NetftUtils::compensateForGravity); 82 | set_max_service = create_service("set_max_values", &NetftUtils::setMax); 83 | threshold_service = create_service("set_threshold", &NetftUtils::setThreshold); 84 | weight_bias_service = create_service("set_weight_bias", &NetftUtils::setWeightBias); 85 | get_weight_service = create_service("get_weight", &NetftUtils::getWeight); 86 | filter_service = create_service("filter", &NetftUtils::setFilter); 87 | } 88 | 89 | void NetftUtils::setUserInput(std::string world, std::string ft, double force, double torque) 90 | { 91 | world_frame = world; 92 | ft_frame = ft; 93 | if(force != 0.0) 94 | { 95 | forceMaxU = force; 96 | } 97 | if(torque != 0.0) 98 | { 99 | torqueMaxU = torque; 100 | } 101 | } 102 | 103 | void NetftUtils::update() 104 | { 105 | // Check for a filter 106 | if(newFilter) 107 | { 108 | delete lp; 109 | lp = new LPFilter(deltaTFilter,cutoffFrequency,6); 110 | newFilter = false; 111 | } 112 | // Look up transform from ft to world frame 113 | geometry_msgs::msg::TransformStamped tempTransform; 114 | tf2::TimePoint time; 115 | 116 | try 117 | { 118 | tempTransform = bufferCore.lookupTransform(world_frame, ft_frame, time); 119 | // listener->waitForTransform(world_frame, ft_frame, ros::Time(0), ros::Duration(1.0)); 120 | // listener->lookupTransform(world_frame, ft_frame, ros::Time(0), tempTransform); 121 | } 122 | catch (tf2::TransformException ex) 123 | { 124 | RCLCPP_ERROR(get_logger(), ex.what()); 125 | } 126 | 127 | // Set translation to zero before updating value 128 | tf2::convert(tempTransform, ft_to_world); 129 | ft_to_world.setOrigin(tf2::Vector3(0.0,0.0,0.0)); 130 | 131 | checkMaxForce(); 132 | 133 | // Publish transformed dat 134 | netft_raw_world_data_pub->publish( raw_data_world ); 135 | netft_world_data_pub->publish( tf_data_world ); 136 | netft_tool_data_pub->publish( tf_data_tool ); 137 | netft_cancel_pub->publish( cancel_msg ); 138 | 139 | rclcpp::spin_some(this->make_shared()); 140 | } 141 | 142 | void NetftUtils::copyWrench(geometry_msgs::msg::WrenchStamped &in, geometry_msgs::msg::WrenchStamped &out, geometry_msgs::msg::WrenchStamped &bias) 143 | { 144 | out.header.stamp = in.header.stamp; 145 | out.header.frame_id = in.header.frame_id; 146 | out.wrench.force.x = in.wrench.force.x - bias.wrench.force.x; 147 | out.wrench.force.y = in.wrench.force.y - bias.wrench.force.y; 148 | out.wrench.force.z = in.wrench.force.z - bias.wrench.force.z; 149 | out.wrench.torque.x = in.wrench.torque.x - bias.wrench.torque.x; 150 | out.wrench.torque.y = in.wrench.torque.y - bias.wrench.torque.y; 151 | out.wrench.torque.z = in.wrench.torque.z - bias.wrench.torque.z; 152 | } 153 | 154 | void NetftUtils::applyThreshold(double &value, double thresh) 155 | { 156 | if(value <= thresh && value >= -thresh) 157 | { 158 | value = 0.0; 159 | } 160 | } 161 | 162 | void NetftUtils::transformFrame(geometry_msgs::msg::WrenchStamped in_data, geometry_msgs::msg::WrenchStamped &out_data, char target_frame) 163 | { 164 | tf2::Vector3 tempF; 165 | tf2::Vector3 tempT; 166 | tempF.setX(in_data.wrench.force.x); 167 | tempF.setY(in_data.wrench.force.y); 168 | tempF.setZ(in_data.wrench.force.z); 169 | tempT.setX(in_data.wrench.torque.x); 170 | tempT.setY(in_data.wrench.torque.y); 171 | tempT.setZ(in_data.wrench.torque.z); 172 | if(target_frame == 'w') 173 | { 174 | out_data.header.frame_id = world_frame; 175 | tempF = ft_to_world * tempF; 176 | tempT = ft_to_world * tempT; 177 | } 178 | else if(target_frame == 't') 179 | { 180 | out_data.header.frame_id = ft_frame; 181 | tempF = ft_to_world.inverse() * tempF; 182 | tempT = ft_to_world.inverse() * tempT; 183 | } 184 | out_data.header.stamp = in_data.header.stamp; 185 | out_data.wrench.force.x = tempF.getX(); 186 | out_data.wrench.force.y = tempF.getY(); 187 | out_data.wrench.force.z = tempF.getZ(); 188 | out_data.wrench.torque.x = tempT.getX(); 189 | out_data.wrench.torque.y = tempT.getY(); 190 | out_data.wrench.torque.z = tempT.getZ(); 191 | } 192 | 193 | // Runs when a new datapoint comes in 194 | void NetftUtils::netftCallback(const geometry_msgs::msg::WrenchStamped::ConstPtr& data) 195 | { 196 | // Filter data 197 | std::vector tempData; 198 | tempData.resize(6); 199 | tempData.at(0) = -data->wrench.force.x; 200 | tempData.at(1) = data->wrench.force.y; 201 | tempData.at(2) = data->wrench.force.z; 202 | tempData.at(3) = -data->wrench.torque.x; 203 | tempData.at(4) = data->wrench.torque.y; 204 | tempData.at(5) = data->wrench.torque.z; 205 | 206 | if(isFilterOn && !newFilter) 207 | lp->update(tempData,tempData); 208 | 209 | // Copy tool frame data. apply negative to x data to follow right hand rule convention (ft raw data does not) 210 | raw_data_tool.header.stamp = data->header.stamp; 211 | raw_data_tool.header.frame_id = ft_frame; 212 | raw_data_tool.wrench.force.x = tempData.at(0); 213 | raw_data_tool.wrench.force.y = tempData.at(1); 214 | raw_data_tool.wrench.force.z = tempData.at(2); 215 | raw_data_tool.wrench.torque.x = tempData.at(3); 216 | raw_data_tool.wrench.torque.y = tempData.at(4); 217 | raw_data_tool.wrench.torque.z = tempData.at(5); 218 | 219 | // Copy in new netft data in tool frame and transform to world frame 220 | transformFrame(raw_data_tool, raw_data_world, 'w'); 221 | 222 | if (isGravityBiased) // Compensate for gravity. Assumes world Z-axis is up 223 | { 224 | // Gravity moment = (payloadLeverArm) cross (payload force) <== all in the sensor frame. Need to convert to world later 225 | // Since it's assumed that the CoM of the payload is on the sensor's central axis, this calculation is simplified. 226 | double gravMomentX = -payloadLeverArm*tf_data_tool.wrench.force.y; 227 | double gravMomentY = payloadLeverArm*tf_data_tool.wrench.force.x; 228 | 229 | // Subtract the gravity torques from the previously-calculated wrench in the tool frame 230 | tf_data_tool.wrench.torque.x = tf_data_tool.wrench.torque.x - gravMomentX; 231 | tf_data_tool.wrench.torque.y = tf_data_tool.wrench.torque.y - gravMomentY; 232 | 233 | // Convert to world to account for the gravity force. Assumes world-Z is up. 234 | //ROS_INFO_STREAM("gravity force in the world Z axis: "<< payloadWeight); 235 | transformFrame(tf_data_tool, tf_data_world, 'w'); 236 | tf_data_world.wrench.force.z = tf_data_world.wrench.force.z - payloadWeight; 237 | 238 | // tf_data_world now accounts for gravity completely. Convert back to the tool frame to make that data available, too 239 | transformFrame(tf_data_world, tf_data_tool, 't'); 240 | } 241 | 242 | if (isBiased) // Apply the bias for a static sensor frame 243 | { 244 | // Get tool bias in world frame 245 | geometry_msgs::msg::WrenchStamped world_bias; 246 | transformFrame(bias, world_bias, 'w'); 247 | // Add bias and apply threshold to get transformed data 248 | copyWrench(raw_data_world, tf_data_world, world_bias); 249 | } 250 | else // Just pass the data straight through 251 | { 252 | copyWrench(raw_data_world, tf_data_world, zero_wrench); 253 | copyWrench(raw_data_tool, tf_data_tool, zero_wrench); 254 | } 255 | 256 | // Apply thresholds 257 | applyThreshold(tf_data_world.wrench.force.x, threshold.wrench.force.x); 258 | applyThreshold(tf_data_world.wrench.force.y, threshold.wrench.force.y); 259 | applyThreshold(tf_data_world.wrench.force.z, threshold.wrench.force.z); 260 | applyThreshold(tf_data_world.wrench.torque.x, threshold.wrench.torque.x); 261 | applyThreshold(tf_data_world.wrench.torque.y, threshold.wrench.torque.y); 262 | applyThreshold(tf_data_world.wrench.torque.z, threshold.wrench.torque.z); 263 | applyThreshold(tf_data_tool.wrench.force.x, threshold.wrench.force.x); 264 | applyThreshold(tf_data_tool.wrench.force.y, threshold.wrench.force.y); 265 | applyThreshold(tf_data_tool.wrench.force.z, threshold.wrench.force.z); 266 | applyThreshold(tf_data_tool.wrench.torque.x, threshold.wrench.torque.x); 267 | applyThreshold(tf_data_tool.wrench.torque.y, threshold.wrench.torque.y); 268 | applyThreshold(tf_data_tool.wrench.torque.z, threshold.wrench.torque.z); 269 | //ROS_INFO_STREAM("Callback time: " << tf_data_tool.header.stamp.toSec()-ros::Time::now().toSec()); 270 | } 271 | 272 | // Set the readings from the sensor to zero at this instant and continue to apply the bias on future readings. 273 | // This doesn't account for gravity. 274 | // Useful when the sensor's orientation won't change. 275 | // Run this method when the sensor is stationary to avoid inertial effects. 276 | // Cannot bias the sensor if gravity compensation has already been applied. 277 | bool NetftUtils::fixedOrientationBias(netft_interfaces::srv::SetBias::Request &req, netft_interfaces::srv::SetBias::Response &res) 278 | { 279 | if(req.bias) 280 | { 281 | copyWrench(raw_data_tool, bias, zero_wrench); // Store the current wrench readings in the 'bias' variable, to be applied hereafter 282 | if(req.force >= 0.0001) // if forceMax was specified and > 0 283 | forceMaxB = req.force; 284 | if(req.torque >= 0.0001) 285 | torqueMaxB = req.torque; // if torqueMax was specified and > 0 286 | 287 | isNewBias = true; 288 | isBiased = true; 289 | } 290 | else 291 | { 292 | copyWrench(zero_wrench, bias, zero_wrench); // Clear the stored bias if the argument was false 293 | } 294 | 295 | res.success = true; 296 | 297 | return true; 298 | } 299 | 300 | // Calculate the payload's mass and center of mass so gravity can be compensated for, even as the sensor changes orientation. 301 | // It's assumed that the payload's center of mass is located on the sensor's central access. 302 | // Run this method when the sensor is stationary to avoid inertial effects. 303 | // Cannot do gravity compensation if sensor has already been biased. 304 | bool NetftUtils::compensateForGravity(netft_interfaces::srv::SetBias::Request &req, netft_interfaces::srv::SetBias::Response &res) 305 | { 306 | 307 | if(req.bias) 308 | { 309 | if (isBiased) 310 | { 311 | RCLCPP_ERROR(get_logger(), "Cannot compensate for gravity if the sensor has already been biased, i.e. useful data was wiped out"); 312 | res.success = false; 313 | return false; 314 | } 315 | else // Cannot compensate for gravity if the sensor has already been biased, i.e. useful data was wiped out 316 | { 317 | // Get the weight of the payload. Assumes the world Z axis is up. 318 | payloadWeight = raw_data_world.wrench.force.z; 319 | 320 | // Calculate the z-coordinate of the payload's center of mass, in the sensor frame. 321 | // It's assumed that the x- and y-coordinates are zero. 322 | // This is a lever arm. 323 | payloadLeverArm = raw_data_tool.wrench.torque.y/raw_data_tool.wrench.force.x; 324 | 325 | isNewGravityBias = true; 326 | isGravityBiased = true; 327 | } 328 | } 329 | 330 | res.success = true; 331 | return true; 332 | } 333 | 334 | bool NetftUtils::setFilter(netft_interfaces::srv::SetFilter::Request &req, netft_interfaces::srv::SetFilter::Response &res) 335 | { 336 | if(req.filter) 337 | { 338 | newFilter = true; 339 | isFilterOn = true; 340 | deltaTFilter = req.t; 341 | cutoffFrequency = req.cutoff_frequency; 342 | } 343 | else 344 | { 345 | isFilterOn = false; 346 | } 347 | 348 | return true; 349 | } 350 | 351 | bool NetftUtils::setMax(netft_interfaces::srv::SetMax::Request &req, netft_interfaces::srv::SetMax::Response &res) 352 | { 353 | 354 | if(req.force >= 0.0001) 355 | forceMaxU = req.force; 356 | if(req.torque >= 0.0001) 357 | torqueMaxU = req.torque; 358 | 359 | res.success = true; 360 | return true; 361 | } 362 | 363 | bool NetftUtils::setWeightBias(netft_interfaces::srv::SetBias::Request &req, netft_interfaces::srv::SetBias::Response &res) 364 | { 365 | if(req.bias) 366 | { 367 | copyWrench(raw_data_tool, weight_bias, zero_wrench); 368 | } 369 | else 370 | { 371 | copyWrench(zero_wrench, weight_bias, zero_wrench); 372 | } 373 | res.success = true; 374 | 375 | return true; 376 | } 377 | 378 | bool NetftUtils::getWeight(netft_interfaces::srv::GetDouble::Request &req, netft_interfaces::srv::GetDouble::Response &res) 379 | { 380 | geometry_msgs::msg::WrenchStamped carried_weight; 381 | copyWrench(raw_data_tool, carried_weight, weight_bias); 382 | res.weight = pow((pow(carried_weight.wrench.force.x, 2.0) + pow(carried_weight.wrench.force.y, 2.0) + pow(carried_weight.wrench.force.z, 2.0)), 0.5)/9.81*1000; 383 | 384 | return true; 385 | } 386 | 387 | bool NetftUtils::setThreshold(netft_interfaces::srv::SetThreshold::Request &req, netft_interfaces::srv::SetThreshold::Response &res) 388 | { 389 | threshold.wrench.force.x = req.data.wrench.force.x; 390 | threshold.wrench.force.y = req.data.wrench.force.y; 391 | threshold.wrench.force.z = req.data.wrench.force.z; 392 | threshold.wrench.torque.x = req.data.wrench.torque.x; 393 | threshold.wrench.torque.y = req.data.wrench.torque.y; 394 | threshold.wrench.torque.z = req.data.wrench.torque.z; 395 | 396 | res.success = true; 397 | 398 | return true; 399 | } 400 | 401 | void NetftUtils::checkMaxForce() 402 | { 403 | double fMag = pow((pow(tf_data_tool.wrench.force.x, 2.0) + pow(tf_data_tool.wrench.force.y, 2.0) + pow(tf_data_tool.wrench.force.z, 2.0)), 0.5); 404 | double tMag = pow((pow(tf_data_tool.wrench.torque.x, 2.0) + pow(tf_data_tool.wrench.torque.y, 2.0) + pow(tf_data_tool.wrench.torque.z, 2.0)), 0.5); 405 | double fMax; 406 | double tMax; 407 | if(isBiased && !isNewBias) 408 | { 409 | fMax = forceMaxB; 410 | tMax = torqueMaxB; 411 | } 412 | else 413 | { 414 | if(isBiased && isNewBias) 415 | { 416 | isNewBias = false; 417 | } 418 | 419 | fMax = forceMaxU; //50.0; 420 | tMax = torqueMaxU; //5.0; 421 | } 422 | 423 | // If max FT exceeded, send cancel unless we have just sent it MAX_CANCEL times 424 | //ROS_INFO("FMAG: %f TMAG: %f", fMag, tMag); 425 | if((fabs(fMag) > fMax || fabs(tMag) > tMax) && cancel_count > 0) 426 | { 427 | cancel_msg.cancel = true; 428 | //ROS_INFO("Force torque violation. Canceling move."); 429 | RCLCPP_INFO(get_logger(), "FMAG: %f FMAX: %f TMAG:%f TMAX: %f count: %d wait: %d", fMag, fMax, tMag, tMax, cancel_count, cancel_wait); 430 | cancel_count-=1; 431 | } 432 | // If we have sent cancel MAX_CANCEL times, don't send cancel again for MAX_WAIT cycles 433 | else if(cancel_count == 0 && cancel_wait > 0 && cancel_wait <= MAX_WAIT) 434 | { 435 | cancel_msg.cancel = false; 436 | cancel_wait-=1; 437 | } 438 | // If we have just finished waiting MAX_WAIT times, or the max force is no longer exceeded, reset cancel_count and cancel_wait 439 | else if(cancel_wait == 0 || !(fabs(fMag) > fMax || fabs(tMag) > tMax)) 440 | { 441 | cancel_msg.cancel = false; 442 | cancel_count = MAX_CANCEL; 443 | cancel_wait = MAX_WAIT; 444 | } 445 | } 446 | 447 | 448 | int main(int argc, char **argv) 449 | { 450 | // Initialize the ros netft_utils_node 451 | rclcpp::init(argc, argv); 452 | 453 | // Instantiate utils class 454 | NetftUtils utils; 455 | using rclcpp::executors::MultiThreadedExecutor; 456 | MultiThreadedExecutor exec; 457 | exec.spin(); 458 | 459 | // Initialize utils 460 | utils.initialize(); 461 | 462 | // Set up user input 463 | std::string world_frame; 464 | std::string ft_frame; 465 | double forceMaxU = 0.0; 466 | double torqueMaxU = 0.0; 467 | if(argc<3) 468 | { 469 | RCLCPP_FATAL(utils.getLog(), "You must pass in at least the world and ft frame as command line arguments. Argument options are [world frame, ft frame, max force, max torque]"); 470 | return 1; 471 | } 472 | else if(argc>=6) 473 | { 474 | RCLCPP_FATAL(utils.getLog(), "Too many arguments for netft_utils"); 475 | } 476 | else 477 | { 478 | world_frame = argv[1]; 479 | ft_frame = argv[2]; 480 | if(argc>=4) 481 | forceMaxU = atof(argv[3]); 482 | if(5==argc) 483 | torqueMaxU = atof(argv[4]); 484 | } 485 | utils.setUserInput(world_frame, ft_frame, forceMaxU, torqueMaxU); 486 | 487 | // Main ros loop 488 | rclcpp::Rate loop_rate(500); 489 | //ros::Time last; 490 | while(rclcpp::ok()) 491 | { 492 | utils.update(); 493 | loop_rate.sleep(); 494 | //ros::Time curr = ros::Time::now(); 495 | //ROS_INFO_STREAM("Loop time: " << curr.toSec()-last.toSec()); 496 | //last = curr; 497 | } 498 | 499 | return 0; 500 | } 501 | 502 | } 503 | -------------------------------------------------------------------------------- /netft_utils/src/netft_utils_lean.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | Copyright (c) 2016, Los Alamos National Security, LLC 4 | All rights reserved. 5 | Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL. 6 | 7 | Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | 12 | THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | 14 | Author: Alex von Sternberg 15 | */ 16 | 17 | #include "netft_utils_lean.h" 18 | namespace netft_utils_lean 19 | { 20 | 21 | NetftUtilsLean::NetftUtilsLean() : Node("netft_utils_lean"), 22 | ftAddress(""), 23 | ftTopic(""), 24 | isInit(false), 25 | hasData(false), 26 | cycleRate(0.0), 27 | isFilterOn(false), 28 | deltaTFilter(0.0), 29 | cutoffFrequency(0.0), 30 | newFilter(false), 31 | lpExists(false), 32 | isBiased(false), 33 | isNewBias(false), 34 | waitingForTransform(true), 35 | isActive(false), 36 | cancel_count(MAX_CANCEL), 37 | cancel_wait(MAX_WAIT), 38 | forceMaxB(10.0), 39 | torqueMaxB(0.8), 40 | forceMaxU(50.0), 41 | torqueMaxU(5.0), 42 | toUpdate(false), 43 | toMonitor(false) 44 | { 45 | } 46 | 47 | NetftUtilsLean::~NetftUtilsLean() 48 | { 49 | delete listener; 50 | if(lpExists) 51 | delete lp; 52 | } 53 | 54 | bool NetftUtilsLean::initialize(double rate, std::string world, std::string ft, double force, double torque) 55 | { 56 | // Set the user input 57 | if(!setUserInput(world, ft, force, torque)) 58 | { 59 | return false; 60 | } 61 | // Set the cycle rate 62 | if(rate>0.001) 63 | { 64 | cycleRate = rate; 65 | } 66 | else 67 | { 68 | RCLCPP_ERROR_STREAM(get_logger(), "Cycle rate should be positive"); 69 | return false; 70 | } 71 | 72 | // Set up access to netft data 73 | if(!ftTopic.empty()) 74 | { 75 | ft_sub = this->create_subscription(ftTopic, 1, &NetftUtilsLean::dataCallback); 76 | RCLCPP_INFO_STREAM(get_logger(), "Using NETFT topic instead of IP because ftTopic is:" << ftTopic); 77 | } 78 | else if(!ftAddress.empty()) 79 | { 80 | try 81 | { 82 | netft = std::unique_ptr(new netft_rdt_driver::NetFTRDTDriver(ftAddress)); 83 | } 84 | catch(std::runtime_error) 85 | { 86 | RCLCPP_ERROR_STREAM(get_logger(), "Could not access data from netft_rdt_driver"); 87 | return false; 88 | } 89 | } 90 | else 91 | { 92 | RCLCPP_ERROR_STREAM(get_logger(), "Can not initialize FT, must set topic or address first."); 93 | return false; 94 | } 95 | 96 | //Zero out the zero wrench 97 | zero_wrench.wrench.force.x = 0.0; 98 | zero_wrench.wrench.force.y = 0.0; 99 | zero_wrench.wrench.force.z = 0.0; 100 | zero_wrench.wrench.torque.x = 0.0; 101 | zero_wrench.wrench.torque.y = 0.0; 102 | zero_wrench.wrench.torque.z = 0.0; 103 | 104 | //Initialize cancel message 105 | cancel_msg.cancel = false; 106 | 107 | //Listen to the transfomation from the ft sensor to world frame. 108 | listener = new tf2_ros::TransformListener(bufferCore); 109 | 110 | //Publish on the /cancel topic. Queue up to 100000 data points 111 | netft_cancel_pub = this->create_publisher("/netft/cancel", 100000); 112 | 113 | if(DEBUG_DATA) 114 | data_pub = this->create_publisher("/netft/netft_data", 100000); 115 | 116 | isInit = true; 117 | return true; 118 | } 119 | 120 | bool NetftUtilsLean::run() 121 | { 122 | if(!isInit) 123 | { 124 | RCLCPP_ERROR_STREAM(get_logger(), "Cannot run before initialization is successful."); 125 | return false; 126 | } 127 | 128 | isActive = true; 129 | 130 | toUpdate = true; 131 | toMonitor = true; 132 | 133 | //Spin off update thread 134 | updateThread = std::async(std::launch::async, &NetftUtilsLean::update, this); 135 | 136 | //Spin off thread to monitor netft data 137 | if(!ftAddress.empty()) 138 | { 139 | monitorThread = std::async(std::launch::async, &NetftUtilsLean::monitorData, this); 140 | } 141 | 142 | //Join threads 143 | if(!ftAddress.empty()) 144 | { 145 | monitorThread.get(); 146 | } 147 | updateThread.get(); 148 | 149 | isActive = false; 150 | return true; 151 | } 152 | 153 | void NetftUtilsLean::stop() 154 | { 155 | toUpdate = false; 156 | toMonitor = false; 157 | } 158 | 159 | bool NetftUtilsLean::monitorData() 160 | { 161 | while(waitingForTransform) 162 | { 163 | std::chrono::nanoseconds duration(5000); 164 | rclcpp::sleep_for(duration); 165 | } 166 | rclcpp::Rate r(cycleRate); 167 | while (rclcpp::ok() && toMonitor) 168 | { 169 | //ros::Time tempTime = ros::Time::now(); 170 | if (netft->waitForNewData()) 171 | { 172 | //std::cout << "Time waiting for new data: " <getData(data); 176 | //std::cout << "Time to get data: " <header; 197 | raw_topic_data.wrench = msg->wrench; 198 | netftCallback(raw_topic_data); 199 | hasData = true; 200 | } 201 | else 202 | hasData = false; 203 | } 204 | 205 | bool NetftUtilsLean::isReady() 206 | { 207 | return hasData; 208 | } 209 | 210 | bool NetftUtilsLean::waitForData(double timeout) 211 | { 212 | rclcpp::Clock clock; 213 | rclcpp::Time startWait = clock.now(); 214 | while((clock.now().seconds() - startWait.seconds()) < timeout && !hasData) 215 | { 216 | std::chrono::nanoseconds duration(1000); 217 | rclcpp::sleep_for(duration); 218 | } 219 | return hasData; 220 | } 221 | 222 | bool NetftUtilsLean::setUserInput(std::string world, std::string ft, double force, double torque) 223 | { 224 | if(world.compare("") == 0) 225 | { 226 | RCLCPP_ERROR_STREAM(get_logger(), "World frame string cannot be empty"); 227 | return false; 228 | } 229 | world_frame = world; 230 | RCLCPP_INFO_STREAM(get_logger(), "World frame: " << world_frame); 231 | if(ft.compare("") == 0) 232 | { 233 | RCLCPP_ERROR_STREAM(get_logger(), "FT frame string cannot be empty"); 234 | return false; 235 | } 236 | ft_frame = ft; 237 | RCLCPP_INFO_STREAM(get_logger(), "FT frame: " << ft_frame); 238 | if(force >= 0.001) 239 | { 240 | forceMaxU = force; 241 | } 242 | else 243 | { 244 | RCLCPP_ERROR_STREAM(get_logger(), "FT max force must be positive"); 245 | return false; 246 | } 247 | if(torque >= 0.001) 248 | { 249 | torqueMaxU = torque; 250 | } 251 | else 252 | { 253 | RCLCPP_ERROR_STREAM(get_logger(), "FT max torque must be positive"); 254 | return false; 255 | } 256 | return true; 257 | } 258 | 259 | bool NetftUtilsLean::update() 260 | { 261 | if(!isInit) 262 | { 263 | RCLCPP_ERROR_STREAM(get_logger(), "Cannot update until NetftUtilsLean is initialized properly."); 264 | return false; 265 | } 266 | rclcpp::Rate r(cycleRate); 267 | while (rclcpp::ok() && toUpdate) 268 | { 269 | // Check for a filter 270 | if(newFilter) 271 | { 272 | if(lpExists) 273 | delete lp; 274 | lp = new LPFilter(deltaTFilter,cutoffFrequency,6); 275 | lpExists = true; 276 | newFilter = false; 277 | } 278 | 279 | // Look up transform from ft to world frame 280 | geometry_msgs::msg::TransformStamped tempTransform; 281 | try 282 | { 283 | tf2::TimePoint time; 284 | tempTransform = bufferCore.lookupTransform(world_frame, ft_frame, time); 285 | } 286 | catch (tf2::TransformException ex) 287 | { 288 | RCLCPP_ERROR(get_logger(), "%s",ex.what()); 289 | } 290 | 291 | // Set translation to zero before updating value 292 | tf2::convert(tempTransform, ft_to_world); 293 | ft_to_world.setOrigin(tf2::Vector3(0.0,0.0,0.0)); 294 | waitingForTransform = false; 295 | 296 | checkMaxForce(); 297 | 298 | // Publish cancel_msg 299 | netft_cancel_pub->publish( cancel_msg ); 300 | r.sleep(); 301 | } 302 | return true; 303 | } 304 | 305 | void NetftUtilsLean::copyWrench(geometry_msgs::msg::WrenchStamped &in, geometry_msgs::msg::WrenchStamped &out, geometry_msgs::msg::WrenchStamped &diff) 306 | { 307 | out.header.stamp = in.header.stamp; 308 | out.header.frame_id = in.header.frame_id; 309 | out.wrench.force.x = in.wrench.force.x - diff.wrench.force.x; 310 | out.wrench.force.y = in.wrench.force.y - diff.wrench.force.y; 311 | out.wrench.force.z = in.wrench.force.z - diff.wrench.force.z; 312 | out.wrench.torque.x = in.wrench.torque.x - diff.wrench.torque.x; 313 | out.wrench.torque.y = in.wrench.torque.y - diff.wrench.torque.y; 314 | out.wrench.torque.z = in.wrench.torque.z - diff.wrench.torque.z; 315 | } 316 | 317 | void NetftUtilsLean::applyThreshold(double &value, double thresh) 318 | { 319 | if(value <= thresh && value >= -thresh) 320 | { 321 | value = 0.0; 322 | } 323 | } 324 | 325 | void NetftUtilsLean::transformFrame(geometry_msgs::msg::WrenchStamped in_data, geometry_msgs::msg::WrenchStamped &out_data, char target_frame) 326 | { 327 | tf2::Vector3 tempF; 328 | tf2::Vector3 tempT; 329 | tempF.setX(in_data.wrench.force.x); 330 | tempF.setY(in_data.wrench.force.y); 331 | tempF.setZ(in_data.wrench.force.z); 332 | tempT.setX(in_data.wrench.torque.x); 333 | tempT.setY(in_data.wrench.torque.y); 334 | tempT.setZ(in_data.wrench.torque.z); 335 | if(target_frame == 'w') 336 | { 337 | out_data.header.frame_id = world_frame; 338 | tempF = ft_to_world * tempF; 339 | tempT = ft_to_world * tempT; 340 | } 341 | else if(target_frame == 't') 342 | { 343 | out_data.header.frame_id = ft_frame; 344 | tempF = ft_to_world.inverse() * tempF; 345 | tempT = ft_to_world.inverse() * tempT; 346 | } 347 | out_data.header.stamp = in_data.header.stamp; 348 | out_data.wrench.force.x = tempF.getX(); 349 | out_data.wrench.force.y = tempF.getY(); 350 | out_data.wrench.force.z = tempF.getZ(); 351 | out_data.wrench.torque.x = tempT.getX(); 352 | out_data.wrench.torque.y = tempT.getY(); 353 | out_data.wrench.torque.z = tempT.getZ(); 354 | } 355 | 356 | void NetftUtilsLean::netftCallback(const geometry_msgs::msg::WrenchStamped& data) 357 | { 358 | // Filter data. apply negative to x data to follow right hand rule convention (ft raw data does not) 359 | std::vector tempData; 360 | tempData.resize(6); 361 | if(!ftAddress.empty()) 362 | { 363 | tempData.at(0) = -data.wrench.force.x; 364 | tempData.at(1) = data.wrench.force.y; 365 | tempData.at(2) = data.wrench.force.z; 366 | tempData.at(3) = -data.wrench.torque.x; 367 | tempData.at(4) = data.wrench.torque.y; 368 | tempData.at(5) = data.wrench.torque.z; 369 | } 370 | else 371 | { 372 | tempData.at(0) = data.wrench.force.x; 373 | tempData.at(1) = data.wrench.force.y; 374 | tempData.at(2) = data.wrench.force.z; 375 | tempData.at(3) = data.wrench.torque.x; 376 | tempData.at(4) = data.wrench.torque.y; 377 | tempData.at(5) = data.wrench.torque.z; 378 | } 379 | 380 | if(isFilterOn && !newFilter) 381 | lp->update(tempData,tempData); 382 | 383 | // Copy tool frame data. 384 | raw_data_tool.header.stamp = data.header.stamp; 385 | raw_data_tool.header.frame_id = ft_frame; 386 | raw_data_tool.wrench.force.x = tempData.at(0); 387 | raw_data_tool.wrench.force.y = tempData.at(1); 388 | raw_data_tool.wrench.force.z = tempData.at(2); 389 | raw_data_tool.wrench.torque.x = tempData.at(3); 390 | raw_data_tool.wrench.torque.y = tempData.at(4); 391 | raw_data_tool.wrench.torque.z = tempData.at(5); 392 | 393 | // Apply bias 394 | copyWrench(raw_data_tool, tf_data_tool, bias); 395 | 396 | // Copy in new netft data in tool frame and transform to world frame 397 | transformFrame(tf_data_tool, tf_data_world, 'w'); 398 | 399 | // Apply thresholds 400 | applyThreshold(tf_data_world.wrench.force.x, threshold.wrench.force.x); 401 | applyThreshold(tf_data_world.wrench.force.y, threshold.wrench.force.y); 402 | applyThreshold(tf_data_world.wrench.force.z, threshold.wrench.force.z); 403 | applyThreshold(tf_data_world.wrench.torque.x, threshold.wrench.torque.x); 404 | applyThreshold(tf_data_world.wrench.torque.y, threshold.wrench.torque.y); 405 | applyThreshold(tf_data_world.wrench.torque.z, threshold.wrench.torque.z); 406 | applyThreshold(tf_data_tool.wrench.force.x, threshold.wrench.force.x); 407 | applyThreshold(tf_data_tool.wrench.force.y, threshold.wrench.force.y); 408 | applyThreshold(tf_data_tool.wrench.force.z, threshold.wrench.force.z); 409 | applyThreshold(tf_data_tool.wrench.torque.x, threshold.wrench.torque.x); 410 | applyThreshold(tf_data_tool.wrench.torque.y, threshold.wrench.torque.y); 411 | applyThreshold(tf_data_tool.wrench.torque.z, threshold.wrench.torque.z); 412 | 413 | // Publish data for debugging 414 | if(DEBUG_DATA) 415 | data_pub->publish(tf_data_tool); 416 | //RCLCPP_INFO_STREAM(get_logger(), "Callback time: " << tf_data_tool.header.stamp.toSec()-ros::Time::now().toSec()); 417 | } 418 | 419 | bool NetftUtilsLean::biasSensor(bool toBias) 420 | { 421 | if(toBias) 422 | { 423 | if(!hasData) 424 | { 425 | geometry_msgs::msg::WrenchStamped data; 426 | if(!ftTopic.empty()) 427 | { 428 | rclcpp::Clock clock; 429 | rclcpp::Time startTime = clock.now(); 430 | while(clock.now().seconds()-startTime.seconds() < 0.1 && !hasData) 431 | { 432 | std::chrono::nanoseconds duration(1000); 433 | rclcpp::sleep_for(duration); 434 | } 435 | if(hasData) 436 | { 437 | data = raw_topic_data; 438 | } 439 | else 440 | { 441 | RCLCPP_ERROR(this->get_logger(), "Bias sensor failed."); 442 | return false; 443 | } 444 | } 445 | else 446 | { 447 | if (netft->waitForNewData()) 448 | { 449 | netft->getData(data); 450 | } 451 | else 452 | { 453 | RCLCPP_ERROR(this->get_logger(), "Bias sensor failed."); 454 | return false; 455 | } 456 | } 457 | // Copy tool frame data. 458 | raw_data_tool.header.stamp = data.header.stamp; 459 | raw_data_tool.header.frame_id = ft_frame; 460 | raw_data_tool.wrench.force.x = -data.wrench.force.x; 461 | raw_data_tool.wrench.force.y = data.wrench.force.y; 462 | raw_data_tool.wrench.force.z = data.wrench.force.z; 463 | raw_data_tool.wrench.torque.x = -data.wrench.torque.x; 464 | raw_data_tool.wrench.torque.y = data.wrench.torque.y; 465 | raw_data_tool.wrench.torque.z = data.wrench.torque.z; 466 | } 467 | copyWrench(raw_data_tool, bias, zero_wrench); 468 | isNewBias = true; 469 | } 470 | else 471 | { 472 | copyWrench(zero_wrench, bias, zero_wrench); 473 | } 474 | isBiased = toBias; 475 | return true; 476 | } 477 | 478 | bool NetftUtilsLean::setFilter(bool toFilter, double deltaT, double cutoffFreq) 479 | { 480 | if(toFilter) 481 | { 482 | newFilter = true; 483 | isFilterOn = true; 484 | deltaTFilter = deltaT; 485 | cutoffFrequency = cutoffFreq; 486 | } 487 | else 488 | { 489 | isFilterOn = false; 490 | } 491 | 492 | return true; 493 | } 494 | 495 | bool NetftUtilsLean::setMax(double fMaxU, double tMaxU, double fMaxB, double tMaxB) 496 | { 497 | if(fMaxU >= 0.0001) 498 | { 499 | forceMaxU = fMaxU; 500 | torqueMaxU = tMaxU; 501 | forceMaxB = fMaxB; 502 | torqueMaxB = tMaxB; 503 | return true; 504 | } 505 | else 506 | { 507 | RCLCPP_ERROR_STREAM(get_logger(), "All maximum FT values must be positive."); 508 | return false; 509 | } 510 | } 511 | 512 | bool NetftUtilsLean::setThreshold(double fThresh, double tThresh) 513 | { 514 | threshold.wrench.force.x = fThresh; 515 | threshold.wrench.force.y = fThresh; 516 | threshold.wrench.force.z = fThresh; 517 | threshold.wrench.torque.x = tThresh; 518 | threshold.wrench.torque.y = tThresh; 519 | threshold.wrench.torque.z = tThresh; 520 | 521 | return true; 522 | } 523 | 524 | void NetftUtilsLean::checkMaxForce() 525 | { 526 | double fMag = pow((pow(tf_data_tool.wrench.force.x, 2.0) + pow(tf_data_tool.wrench.force.y, 2.0) + pow(tf_data_tool.wrench.force.z, 2.0)), 0.5); 527 | double tMag = pow((pow(tf_data_tool.wrench.torque.x, 2.0) + pow(tf_data_tool.wrench.torque.y, 2.0) + pow(tf_data_tool.wrench.torque.z, 2.0)), 0.5); 528 | double fMax; 529 | double tMax; 530 | if(isBiased && !isNewBias) 531 | { 532 | if(forceMaxB > 0.001 && torqueMaxB > 0.001) 533 | { 534 | fMax = forceMaxB; 535 | tMax = torqueMaxB; 536 | } 537 | else 538 | { 539 | fMax = forceMaxU; 540 | tMax = torqueMaxU; 541 | } 542 | } 543 | else 544 | { 545 | if(isBiased && isNewBias) 546 | { 547 | isNewBias = false; 548 | } 549 | fMax = forceMaxU; 550 | tMax = torqueMaxU; 551 | } 552 | 553 | // If max FT exceeded, send cancel unless we have just sent it MAX_CANCEL times 554 | //RCLCPP_INFO("FMAG: %f TMAG: %f", fMag, tMag); 555 | if((fabs(fMag) > fMax || fabs(tMag) > tMax) && cancel_count > 0) 556 | { 557 | cancel_msg.cancel = true; 558 | //RCLCPP_INFO("Force torque violation. Canceling move."); 559 | RCLCPP_INFO(get_logger(), "FMAG: %f FMAX: %f TMAG:%f TMAX: %f count: %d wait: %d", fMag, fMax, tMag, tMax, cancel_count, cancel_wait); 560 | cancel_count-=1; 561 | } 562 | // If we have sent cancel MAX_CANCEL times, don't send cancel again for MAX_WAIT cycles 563 | else if(cancel_count == 0 && cancel_wait > 0 && cancel_wait <= MAX_WAIT) 564 | { 565 | cancel_msg.cancel = false; 566 | cancel_wait-=1; 567 | } 568 | // If we have just finished waiting MAX_WAIT times, or the max force is no longer exceeded, reset cancel_count and cancel_wait 569 | else if(cancel_wait == 0 || !(fabs(fMag) > fMax || fabs(tMag) > tMax)) 570 | { 571 | cancel_msg.cancel = false; 572 | cancel_count = MAX_CANCEL; 573 | cancel_wait = MAX_WAIT; 574 | } 575 | } 576 | 577 | void NetftUtilsLean::setFTAddress(std::string ftAd) 578 | { 579 | ftAddress = ftAd; 580 | } 581 | 582 | void NetftUtilsLean::setFTTopic(std::string ftTop) 583 | { 584 | ftTopic = ftTop; 585 | } 586 | 587 | void NetftUtilsLean::getRawData(geometry_msgs::msg::WrenchStamped& data) 588 | { 589 | data = raw_data_tool; 590 | } 591 | 592 | void NetftUtilsLean::getToolData(geometry_msgs::msg::WrenchStamped& data) 593 | { 594 | data = tf_data_tool; 595 | } 596 | 597 | void NetftUtilsLean::getWorldData(geometry_msgs::msg::WrenchStamped& data) 598 | { 599 | data = tf_data_world; 600 | } 601 | 602 | bool NetftUtilsLean::isRunning() 603 | { 604 | return isActive; 605 | } 606 | } 607 | 608 | -------------------------------------------------------------------------------- /netft_utils/src/netft_utils_sim.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | Copyright (c) 2016, Los Alamos National Security, LLC 4 | All rights reserved. 5 | Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software. NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE. If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL. 6 | 7 | Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 10 | 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | 12 | THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | 14 | Author: Alex von Sternberg 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | /** 24 | * This program simulates the force torque data coming from a FT sensor 25 | */ 26 | 27 | // SIM vars 28 | enum SimType 29 | { 30 | TYPE_ZEROS = 0, 31 | TYPE_LINEAR = 1, 32 | TYPE_EXP = 2, 33 | TYPE_TRIANGLE = 3 34 | }; 35 | enum Dim {DIM_X = 0, 36 | DIM_Y = 1, 37 | DIM_Z = 2, 38 | DIM_RX = 3, 39 | DIM_RY = 4, 40 | DIM_RZ = 5}; 41 | 42 | geometry_msgs::msg::WrenchStamped simWrench; // Wrench containing the simulated data 43 | bool toSim = false; // True if we should output simulated data 44 | SimType simType; // Defines the simulation profile type 45 | Dim simDim; // Dimension to simulate FT in 46 | double simStart; // Time that the simulation started 47 | double simSlope; // Parameter to tweak how fast force increases 48 | double maxForce = 0.0; // Max force to be simulated 49 | double upSlope = true; // True if the triangle wave is on up slope 50 | 51 | void setWrench(double x, double y, double z, double rx, double ry, double rz) 52 | { 53 | simWrench.wrench.force.x = x; 54 | simWrench.wrench.force.y = y; 55 | simWrench.wrench.force.z = z; 56 | simWrench.wrench.torque.x = rx; 57 | simWrench.wrench.torque.y = ry; 58 | simWrench.wrench.torque.z = rz; 59 | } 60 | 61 | void simData(rclcpp::Node &n) 62 | { 63 | if(toSim) 64 | { 65 | double ft = 0.0; 66 | rclcpp::Clock clock; 67 | double deltaT = clock.now().seconds() - simStart; 68 | switch (simType) 69 | { 70 | case TYPE_ZEROS: 71 | break; 72 | case TYPE_LINEAR: 73 | { 74 | if((deltaT * simSlope) < maxForce) 75 | ft = deltaT * simSlope; 76 | else 77 | ft = maxForce; 78 | } 79 | break; 80 | case TYPE_EXP: 81 | { 82 | if((pow(deltaT,2) * simSlope) < maxForce) 83 | ft = pow(deltaT,2) * simSlope; 84 | else 85 | ft = maxForce; 86 | } 87 | break; 88 | case TYPE_TRIANGLE: 89 | { 90 | if((deltaT * simSlope) < maxForce && upSlope) 91 | { 92 | ft = deltaT * simSlope; 93 | } 94 | else if((deltaT * simSlope) >= maxForce && upSlope) 95 | { 96 | ft = maxForce; 97 | upSlope = false; 98 | } 99 | else if(2*maxForce - deltaT*simSlope > 0.0) 100 | { 101 | ft = 2*maxForce - deltaT*simSlope; 102 | } 103 | else 104 | { 105 | ft = 0.0; 106 | } 107 | } 108 | break; 109 | default: 110 | RCLCPP_ERROR(n.get_logger(), "Sim type invalid. Stopping sim."); 111 | toSim = false; 112 | break; 113 | } 114 | switch (simDim) 115 | { 116 | case DIM_X: 117 | setWrench(ft, 0.0, 0.0, 0.0, 0.0, 0.0); 118 | break; 119 | case DIM_Y: 120 | setWrench(0.0, ft, 0.0, 0.0, 0.0, 0.0); 121 | break; 122 | case DIM_Z: 123 | setWrench(0.0, 0.0, ft, 0.0, 0.0, 0.0); 124 | break; 125 | case DIM_RX: 126 | setWrench(0.0, 0.0, 0.0, ft, 0.0, 0.0); 127 | break; 128 | case DIM_RY: 129 | setWrench(0.0, 0.0, 0.0, 0.0, ft, 0.0); 130 | break; 131 | case DIM_RZ: 132 | setWrench(0.0, 0.0, 0.0, 0.0, 0.0, ft); 133 | break; 134 | default: 135 | RCLCPP_ERROR(n.get_logger(), "Dimension invalid. Stopping sim."); 136 | toSim = false; 137 | break; 138 | } 139 | } 140 | else 141 | { 142 | upSlope = true; 143 | setWrench(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); 144 | } 145 | } 146 | 147 | bool startSim(netft_interfaces::srv::StartSim::Request &req, netft_interfaces::srv::StartSim::Response &res) 148 | { 149 | rclcpp::Clock clock; 150 | simStart = clock.now().seconds(); 151 | simDim = (Dim)req.dim; 152 | simType = (SimType)req.type; 153 | simSlope = req.slope; 154 | maxForce = req.force; 155 | toSim = true; 156 | return true; 157 | } 158 | 159 | bool stopSim(netft_interfaces::srv::StopSim::Request &req, netft_interfaces::srv::StopSim::Response &res) 160 | { 161 | toSim = false; 162 | return true; 163 | } 164 | 165 | int main(int argc, char **argv) 166 | { 167 | //Node name 168 | rclcpp::init(argc, argv); 169 | 170 | //Access main ROS system 171 | rclcpp::Node n ("netft_utils_sim"); 172 | 173 | //Publish on the /netft_transformed_data topic. Queue up to 100000 data points 174 | rclcpp::Publisher::SharedPtr netft_data_pub = n.create_publisher("netft_data", 100000); 175 | 176 | //Advertise bias and threshold services 177 | rclcpp::Service::SharedPtr start_service = n.create_service("start_sim", &startSim); 178 | rclcpp::Service::SharedPtr stop_service = n.create_service("stop_sim", &stopSim); 179 | 180 | rclcpp::Rate loop_rate(400); 181 | 182 | //Initialize variables 183 | upSlope = true; 184 | 185 | while ( rclcpp::ok() ) 186 | { 187 | simData(n); 188 | 189 | // Publish transformed dat 190 | netft_data_pub->publish( simWrench ); 191 | 192 | loop_rate.sleep(); 193 | rclcpp::spin_some(n.make_shared()); 194 | } 195 | 196 | return 0; 197 | } 198 | -------------------------------------------------------------------------------- /netft_utils/urdf/example_description.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /netft_utils/urdf/ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | netft_hardware_interface/NetFTHardwareInterface 9 | ${ip_address} 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | --------------------------------------------------------------------------------