From 2b72a4f102281174a9dcf29e191c4ff3892f0e3c Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Thu, 28 Feb 2019 12:56:17 +0100 Subject: [PATCH 1/5] plugins: sim and handler --- .travis.rosinstall | 4 + CMakeLists.txt | 11 +- hardware_interface_plugin.xml | 21 + .../force_torque_sensor_hw.h | 4 +- .../force_torque_sensor_hw_handler.h | 231 +++++++ .../force_torque_sensor_sim.h | 8 +- package.xml | 1 + src/force_torque_sensor_hw.cpp | 57 ++ src/force_torque_sensor_hw_handler.cpp | 629 ++++++++++++++++++ src/force_torque_sensor_sim.cpp | 19 + urdf/fts_sim.urdf.xacro | 51 ++ 11 files changed, 1032 insertions(+), 4 deletions(-) create mode 100644 hardware_interface_plugin.xml create mode 100644 include/force_torque_sensor/force_torque_sensor_hw_handler.h create mode 100644 src/force_torque_sensor_hw.cpp create mode 100644 src/force_torque_sensor_hw_handler.cpp create mode 100644 urdf/fts_sim.urdf.xacro diff --git a/.travis.rosinstall b/.travis.rosinstall index a0f8439..fd34389 100644 --- a/.travis.rosinstall +++ b/.travis.rosinstall @@ -7,3 +7,7 @@ uri: 'https://github.com/iirob/rosparam_handler.git' local-name: rosparam_handler version: master +- git: + uri: 'https://github.com/KITrobotics/ros_control.git' + local-name: ros_control + version: combined_robot_sensor_hw diff --git a/CMakeLists.txt b/CMakeLists.txt index 58fc435..ca3d4d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,7 @@ generate_messages( ################################### catkin_package( INCLUDE_DIRS include - CATKIN_DEPENDS geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy realtime_tools pluginlib tf2 tf2_ros + CATKIN_DEPENDS geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy realtime_tools pluginlib tf2 tf2_ros hardware_interface DEPENDS Eigen3 LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_sim ) @@ -85,7 +85,11 @@ include_directories( ${Eigen3_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} src/force_torque_sensor_handle.cpp ) +add_library(${PROJECT_NAME} + src/force_torque_sensor_handle.cpp + src/force_torque_sensor_hw.cpp + src/force_torque_sensor_hw_handler.cpp +) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg # For dynamic_reconfigure ${PROJECT_NAME}_generate_messages_cpp @@ -120,3 +124,6 @@ install(DIRECTORY config/ cfg/ install(FILES force_torque_sensor_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES hardware_interface_plugin.xml +DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/hardware_interface_plugin.xml b/hardware_interface_plugin.xml new file mode 100644 index 0000000..035cd87 --- /dev/null +++ b/hardware_interface_plugin.xml @@ -0,0 +1,21 @@ + + + + FTS interface + + + + + + Force Torque Sensor Handler + + + + + + + + Simulated Force Torque Sensor + + + diff --git a/include/force_torque_sensor/force_torque_sensor_hw.h b/include/force_torque_sensor/force_torque_sensor_hw.h index f7d04ee..d70faa7 100644 --- a/include/force_torque_sensor/force_torque_sensor_hw.h +++ b/include/force_torque_sensor/force_torque_sensor_hw.h @@ -2,10 +2,12 @@ #ifndef FORCETORQUESENSORHW_INCLUDEDEF_H #define FORCETORQUESENSORHW_INCLUDEDEF_H +#include + namespace hardware_interface { -class ForceTorqueSensorHW +class ForceTorqueSensorHW : public hardware_interface::SensorHW { public: ForceTorqueSensorHW() {}; diff --git a/include/force_torque_sensor/force_torque_sensor_hw_handler.h b/include/force_torque_sensor/force_torque_sensor_hw_handler.h new file mode 100644 index 0000000..f0add91 --- /dev/null +++ b/include/force_torque_sensor/force_torque_sensor_hw_handler.h @@ -0,0 +1,231 @@ +/**************************************************************** + * + * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, + * Institute for Anthropomatics and Robotics (IAR) - + * Intelligent Process Control and Robotics (IPR), + * Karlsruhe Institute of Technology + * + * Maintainers: Denis Štogl, email: denis.stogl@kit.edu + * Florian Heller + * Vanessa Streuer + * + * Date of update: 2014-2016 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#ifndef FORCETORQUESENSORHANDLE_INCLUDEDEF_H +#define FORCETORQUESENSORHANDLE_INCLUDEDEF_H + +#include +#include + +#include +typedef unsigned char uint8_t; +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define PI 3.14159265 + +namespace force_torque_sensor +{ + +class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorHW +{ +public: + ForceTorqueSensorHWHandler(); + + void prepareNode(std::string output_frame); + + void init_sensor(std::string &msg, bool &success); + bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool srvCallback_CalculateOffset(force_torque_sensor::CalculateSensorOffset::Request &req, force_torque_sensor::CalculateSensorOffset::Response &res); + bool srvCallback_CalculateAverageMasurement(force_torque_sensor::CalculateAverageMasurement::Request &req, force_torque_sensor::CalculateAverageMasurement::Response &res); + bool calibrate(bool apply_after_calculation, geometry_msgs::Wrench *new_offset); + bool srvCallback_DetermineCoordinateSystem(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool srvReadDiagnosticVoltages(force_torque_sensor::DiagnosticVoltages::Request &req, + force_torque_sensor::DiagnosticVoltages::Response &res); + bool srvCallback_recalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool srvCallback_setSensorOffset(force_torque_sensor::SetSensorOffset::Request &req, + force_torque_sensor::SetSensorOffset::Response &res); + bool init ( ros::NodeHandle& root_nh, ros::NodeHandle &sensor_hw_nh ); + void read ( const ros::Time& time, const ros::Duration& period ); + +private: + void updateFTData(); + geometry_msgs::Wrench makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id=""); + + bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench& transformed); + + // Arrays for hardware_interface + double interface_force_[3]; + double interface_torque_[3]; + + force_torque_sensor::CoordinateSystemCalibrationParameters CS_params_; + force_torque_sensor::HWCommunicationConfigurationParameters HWComm_params_; + force_torque_sensor::FTSConfigurationParameters FTS_params_; + force_torque_sensor::PublishConfigurationParameters pub_params_; + force_torque_sensor::NodeConfigurationParameters node_params_; + force_torque_sensor::CalibrationParameters calibration_params_; + iirob_filters::GravityCompensationParameters gravity_params_; + + std::string transform_frame_; + std::string sensor_frame_; + + void pullFTData(); + void filterFTData(); + + // Arrays for dumping FT-Data + geometry_msgs::WrenchStamped gravity_compensated_force, moving_mean_filtered_wrench, threshold_filtered_force, transformed_data, sensor_data, low_pass_filtered_data; + + double force_buffer_[3]; + double torque_buffer_[3]; + double force_buffer_transformed_[3]; + double torque_buffer_transformed_[3]; + + ros::NodeHandle nh_; + + //FT Data + hardware_interface::ForceTorqueSensorHW *p_Ftc; + geometry_msgs::Wrench offset_; + geometry_msgs::TransformStamped transform_ee_base_stamped; + tf2_ros::Buffer *p_tfBuffer; + realtime_tools::RealtimePublisher *gravity_compensated_pub_, *threshold_filtered_pub_, *transformed_data_pub_, *sensor_data_pub_, *low_pass_pub_, *moving_mean_pub_; + + bool is_pub_gravity_compensated_ = false; + bool is_pub_threshold_filtered_ = false; + bool is_pub_transformed_data_ = false; + bool is_pub_sensor_data_ = false; + bool is_pub_low_pass_ = false; + bool is_pub_moving_mean_ = false; + + uint _num_transform_errors; + + // HWComm parameters + int HWCommType; // Only important if can is used + std::string HWCommPath; + int HWCommBaudrate; + int ftsBaseID; + double nodePubFreq, nodePullFreq; + uint calibrationNMeasurements; + double calibrationTBetween; + int coordinateSystemNMeasurements; + int coordinateSystemTBetween; + int coordinateSystemPushDirection; + + // service servers + ros::ServiceServer srvServer_Init_; + ros::ServiceServer srvServer_CalculateAverageMasurement_; + ros::ServiceServer srvServer_CalculateOffset_; + ros::ServiceServer srvServer_DetermineCoordianteSystem_; + ros::ServiceServer srvServer_Temp_; + ros::ServiceServer srvServer_ReCalibrate; + ros::ServiceServer srvServer_SetSensorOffset; + + ros::Timer ftUpdateTimer_, ftPullTimer_; + + tf2_ros::TransformListener *p_tfListener; + tf2::Transform transform_ee_base; + + bool m_isInitialized; + bool m_isCalibrated; + bool apply_offset; + + // Variables for Static offset + bool m_staticCalibration; + geometry_msgs::Wrench m_calibOffset; + + + filters::FilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); + filters::FilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); + filters::FilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); + filters::FilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); + + bool useGravityCompensator=false; + bool useThresholdFilter=false; + + bool useMovingMean = false; + bool useLowPassFilter = false; + + + dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service + + void reconfigureCalibrationRequest(force_torque_sensor::CalibrationConfig& config, uint32_t level); + + boost::shared_ptr> sensor_loader_; + boost::shared_ptr sensor_; + + hardware_interface::ForceTorqueSensorInterface fts_interface_; + ros::Time last_publish_time_; + ros::Time last_pull_time_; +}; + +} +#endif + diff --git a/include/force_torque_sensor/force_torque_sensor_sim.h b/include/force_torque_sensor/force_torque_sensor_sim.h index 25ae0aa..13d1f0a 100644 --- a/include/force_torque_sensor/force_torque_sensor_sim.h +++ b/include/force_torque_sensor/force_torque_sensor_sim.h @@ -5,7 +5,7 @@ #include #include #include - +#include #include namespace force_torque_sensor @@ -26,9 +26,15 @@ class ForceTorqueSensorSim : public hardware_interface::ForceTorqueSensorHW void subscribeData(const geometry_msgs::Twist::ConstPtr& msg); geometry_msgs::WrenchStamped joystick_data; + bool init ( ros::NodeHandle& root_nh, ros::NodeHandle &sensor_hw_nh ); + void read ( const ros::Time& time, const ros::Duration& period ); + private: ros::NodeHandle nh_; ros::Subscriber force_input_subscriber; + hardware_interface::ForceTorqueSensorInterface fts_interface_; + double force_[3]; + double torque_[3]; }; } diff --git a/package.xml b/package.xml index 60bcc5d..50bb5b3 100644 --- a/package.xml +++ b/package.xml @@ -35,5 +35,6 @@ + diff --git a/src/force_torque_sensor_hw.cpp b/src/force_torque_sensor_hw.cpp new file mode 100644 index 0000000..878b1a0 --- /dev/null +++ b/src/force_torque_sensor_hw.cpp @@ -0,0 +1,57 @@ +/**************************************************************** + * + * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, + * Institute for Anthropomatics and Robotics (IAR) - + * Intelligent Process Control and Robotics (IPR), + * Karlsruhe Institute of Technology + * + * Maintainers: Denis Štogl, email: denis.stogl@kit.edu + * Andreea Tulbure + * + * Date of update: 2017 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Copyright (c) 2010 + * + * Fraunhofer Institute for Manufacturing Engineering + * and Automation (IPA) + * + * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de + * + * Date of creation: June 2010 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ +#include +#include + +PLUGINLIB_EXPORT_CLASS(hardware_interface::ForceTorqueSensorHW, hardware_interface::SensorHW) diff --git a/src/force_torque_sensor_hw_handler.cpp b/src/force_torque_sensor_hw_handler.cpp new file mode 100644 index 0000000..02bc39c --- /dev/null +++ b/src/force_torque_sensor_hw_handler.cpp @@ -0,0 +1,629 @@ +/**************************************************************** + * + * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, + * Institute for Anthropomatics and Robotics (IAR) - + * Intelligent Process Control and Robotics (IPR), + * Karlsruhe Institute of Technology + * + * Maintainers: Denis Štogl, email: denis.stogl@kit.edu + * Florian Heller + * Vanessa Streuer + * + * Date of update: 2014-2016 + * + * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License LGPL as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License LGPL for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License LGPL along with this program. + * If not, see . + * + ****************************************************************/ + +#include +#include +#include + +using namespace force_torque_sensor; + +ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler() : calibration_params_ {nh_.getNamespace() + "/Calibration/Offset"}, CS_params_ {nh_.getNamespace() }, HWComm_params_ {nh_.getNamespace() + "/HWComm"}, FTS_params_ {nh_.getNamespace() + "/FTS"}, pub_params_ {nh_.getNamespace() + "/Publish"}, node_params_ {nh_.getNamespace() + "/Node"}, gravity_params_ {nh_.getNamespace() + "/GravityCompensation/params"} {} + +bool ForceTorqueSensorHWHandler::init(ros::NodeHandle &root_nh, ros::NodeHandle &sensor_hw_nh) +{ + node_params_.fromParamServer(); + + sensor_loader_.reset(new pluginlib::ClassLoader ("force_torque_sensor", "hardware_interface::ForceTorqueSensorHW")); + if (!node_params_.sensor_hw.empty()) { + try { + sensor_.reset(sensor_loader_->createUnmanagedInstance(node_params_.sensor_hw)); + ROS_INFO_STREAM("Sensor type " << node_params_.sensor_hw << " was successfully loaded."); + + p_Ftc = sensor_.get(); + prepareNode(node_params_.transform_frame); + } catch (pluginlib::PluginlibException &e) { + ROS_ERROR_STREAM("Plugin failed to load:" << e.what()); + } + } else { + ROS_ERROR_STREAM("Failed to getParam 'sensor_hw' (namespace: " << nh_.getNamespace() << ")."); + ROS_ERROR("Sensor hardware failed to load"); + return false; + } + + last_publish_time_ = ros::Time::now(); + last_pull_time_ = ros::Time::now(); + + hardware_interface::ForceTorqueSensorHandle fts_handle(FTS_params_.fts_name, node_params_.sensor_frame, interface_force_, interface_torque_); + fts_interface_.registerHandle(fts_handle); + registerInterface(&fts_interface_); + + return true; +} + +void ForceTorqueSensorHWHandler::read(const ros::Time &time, const ros::Duration &period) +{ + if (nodePullFreq > 0.0 && last_pull_time_ + ros::Duration(1.0 / nodePullFreq) < time) { + pullFTData(); + } + + if (nodePubFreq > 0.0 && last_publish_time_ + ros::Duration(1.0 / nodePubFreq) < time) { + updateFTData(); + } +} + +void ForceTorqueSensorHWHandler::prepareNode(std::string output_frame) +{ + ROS_INFO_STREAM ("Sensor is using namespace '" << nh_.getNamespace() << "'."); + + transform_frame_ = output_frame; + + reconfigCalibrationSrv_.setCallback(boost::bind(&ForceTorqueSensorHWHandler::reconfigureCalibrationRequest, this, _1, _2)); + + calibration_params_.fromParamServer(); + CS_params_.fromParamServer(); + HWComm_params_.fromParamServer(); + FTS_params_.fromParamServer(); + pub_params_.fromParamServer(); + node_params_.fromParamServer(); + gravity_params_.fromParamServer(); + + int calibNMeas; + calibNMeas=calibration_params_.n_measurements; + + if (calibNMeas <= 0) + { + ROS_WARN("Parameter 'Calibration/n_measurements' is %d (<=0) using default: 20", calibNMeas); + calibrationNMeasurements = 20; + } + else { + calibrationNMeasurements = (uint)calibNMeas; + } + calibrationTBetween = calibration_params_.T_between_meas; + m_staticCalibration = calibration_params_.isStatic; + + std::map forceVal,torqueVal; + forceVal = calibration_params_.force; + torqueVal = calibration_params_.torque; + + m_calibOffset.force.x = forceVal["x"]; + m_calibOffset.force.y = forceVal["y"]; + m_calibOffset.force.z = forceVal["z"]; + m_calibOffset.torque.x = torqueVal["x"]; + m_calibOffset.torque.y = torqueVal["y"]; + m_calibOffset.torque.z = torqueVal["z"]; + + bool isAutoInit = false; + m_isInitialized = false; + m_isCalibrated = false; + srvServer_Init_ = nh_.advertiseService("Init", &ForceTorqueSensorHWHandler::srvCallback_Init, this); + srvServer_CalculateAverageMasurement_ = nh_.advertiseService("CalculateAverageMasurement", &ForceTorqueSensorHWHandler::srvCallback_CalculateAverageMasurement, this); + srvServer_CalculateOffset_ = nh_.advertiseService("CalculateOffsets", &ForceTorqueSensorHWHandler::srvCallback_CalculateOffset, this); + srvServer_DetermineCoordianteSystem_ = nh_.advertiseService("DetermineCoordinateSystem", &ForceTorqueSensorHWHandler::srvCallback_DetermineCoordinateSystem, this); + srvServer_Temp_ = nh_.advertiseService("GetTemperature", &ForceTorqueSensorHWHandler::srvReadDiagnosticVoltages, this); + srvServer_ReCalibrate = nh_.advertiseService("Recalibrate", &ForceTorqueSensorHWHandler::srvCallback_recalibrate, this); + srvServer_SetSensorOffset = nh_.advertiseService("SetSensorOffset", &ForceTorqueSensorHWHandler::srvCallback_setSensorOffset, this); + + // Read data from parameter server + HWCommType = HWComm_params_.type; + HWCommPath = HWComm_params_.path; + HWCommBaudrate = HWComm_params_.baudrate; + ftsBaseID = FTS_params_.base_identifier; + isAutoInit = FTS_params_.auto_init; + nodePubFreq = node_params_.ft_pub_freq; + nodePullFreq = node_params_.ft_pull_freq; + sensor_frame_ = node_params_.sensor_frame; + + coordinateSystemNMeasurements = CS_params_.n_measurements; + coordinateSystemTBetween = CS_params_.T_between_meas; + coordinateSystemPushDirection = CS_params_.push_direction; + + p_tfBuffer = new tf2_ros::Buffer(); + p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); + + //Wrench Publisher + is_pub_gravity_compensated_ = pub_params_.gravity_compensated; + if(is_pub_gravity_compensated_){ + gravity_compensated_pub_ = new realtime_tools::RealtimePublisher(nh_, "gravity_compensated", 1); + } + is_pub_low_pass_ = pub_params_.low_pass; + if(is_pub_low_pass_){ + low_pass_pub_ = new realtime_tools::RealtimePublisher(nh_, "low_pass", 1); + } + is_pub_moving_mean_=pub_params_.moving_mean; + if(is_pub_moving_mean_){ + moving_mean_pub_ = new realtime_tools::RealtimePublisher(nh_, "moving_mean", 1); + } + is_pub_sensor_data_=pub_params_.sensor_data; + if(is_pub_sensor_data_){ + sensor_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "sensor_data", 1); + } + is_pub_threshold_filtered_ =pub_params_.threshold_filtered; + if(is_pub_threshold_filtered_){ + threshold_filtered_pub_ = new realtime_tools::RealtimePublisher(nh_, "threshold_filtered", 1); + } + is_pub_transformed_data_ = pub_params_.transformed_data; + if(is_pub_transformed_data_){ + transformed_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "transformed_data", 1); + } + + //Median Filter + if(nh_.hasParam("MovingMeanFilter")) { + useMovingMean = true; + moving_mean_filter_->configure(nh_.getNamespace()+"/MovingMeanFilter"); + } + + //Low Pass Filter + if(nh_.hasParam("LowPassFilter")) { + useLowPassFilter = true; + low_pass_filter_->configure(nh_.getNamespace()+"/LowPassFilter"); + } + + //Gravity Compenstation + if(nh_.hasParam("GravityCompensation")) { + useGravityCompensator = true; + gravity_compensator_->configure(nh_.getNamespace()+"/GravityCompensation"); + } + + //Threshold Filter + if(nh_.hasParam("ThresholdFilter")) { + useThresholdFilter = true; + threshold_filter_->configure(nh_.getNamespace()+"/ThresholdFilter"); + } + + p_Ftc->initCommunication(HWCommType, HWCommPath, HWCommBaudrate, ftsBaseID); + + if (isAutoInit) + { + std::string msg; + bool success; + ROS_INFO("Starting Autoinit..."); + init_sensor(msg, success); + ROS_INFO("Autoinit: %s", msg.c_str()); + } +} + +void ForceTorqueSensorHWHandler::init_sensor(std::string& msg, bool& success) +{ + if (!m_isInitialized) + { + // read return init status and check it! + if (p_Ftc->init()) + { + m_isInitialized = true; + success = true; + msg = "FTS initalised!"; + + // Calibrate sensor + if (m_staticCalibration) + { + ROS_INFO("Using static Calibration Offset from paramter server with parametes Force: x:%f, y:%f, z:%f; Torque: x: %f, y:%f, z:%f;", + m_calibOffset.force.x, m_calibOffset.force.y, m_calibOffset.force.z, + m_calibOffset.torque.x, m_calibOffset.torque.y, m_calibOffset.torque.z + ); + offset_.force.x = m_calibOffset.force.x; + offset_.force.y = m_calibOffset.force.y; + offset_.force.z = m_calibOffset.force.z; + offset_.torque.x= m_calibOffset.torque.x; + offset_.torque.y = m_calibOffset.torque.y; + offset_.torque.z = m_calibOffset.torque.z; + } + else + { + ROS_INFO("Calibrating sensor. Plase wait..."); + geometry_msgs::Wrench temp_offset; + if (not calibrate(true, &temp_offset)) + { + success = false; + msg = "Calibration failed! :/"; + } + } + + apply_offset = true; + + } + else + { + m_isInitialized = false; + success = false; + msg = "FTS could not be initilised! :/"; + } + } +} + +bool ForceTorqueSensorHWHandler::srvCallback_Init(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) +{ + std::string msg; + bool success; + + init_sensor(msg, success); + res.message = msg; + res.success = success; + + return true; +} + +bool ForceTorqueSensorHWHandler::srvCallback_CalculateAverageMasurement(force_torque_sensor::CalculateAverageMasurement::Request& req, force_torque_sensor::CalculateAverageMasurement::Response& res) +{ + if (m_isInitialized) + { + res.success = true; + res.message = "Measurement successfull! :)"; + res.measurement = makeAverageMeasurement(req.N_measurements, req.T_between_meas, req.frame_id); + } + else + { + res.success = false; + res.message = "FTS not initialised! :/"; + } + + return true; +} + +bool ForceTorqueSensorHWHandler::srvCallback_CalculateOffset(force_torque_sensor::CalculateSensorOffset::Request& req, force_torque_sensor::CalculateSensorOffset::Response& res) +{ + if (m_isInitialized) + { + if (calibrate(req.apply_after_calculation, &res.offset)) + { + res.success = true; + res.message = "Calibration successfull! :)"; + } + else + { + res.success = false; + res.message = "Calibration failed! :/"; + } + } + else + { + res.success = false; + res.message = "FTS not initialised! :/"; + } + + return true; +} + +bool ForceTorqueSensorHWHandler::srvCallback_recalibrate(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) +{ + if (!m_isInitialized) + { + ROS_WARN("FTS-Node is not initialized, please initialize first!"); + res.success = false; + res.message = "Failed to recalibrate because Node is not initiliazed."; + return true; + } + if (!(nh_.hasParam("force") && nh_.hasParam("CoG_x") && nh_.hasParam("CoG_y") && nh_.hasParam("CoG_z"))) + { + ROS_ERROR("Cannot use dynamic recalibration without all values for Gravity Compensation, set parameters or use " + "'Calibrate' service instead."); + res.success = false; + res.message = "Failed to recalibrate because of missing Parameters for Gravity Compensation."; + return true; + } + geometry_msgs::Vector3Stamped gravity, gravity_transformed; + geometry_msgs::Vector3 cog; + double force_value; + cog.x = gravity_params_.CoG_x; + cog.y = gravity_params_.CoG_y; + cog.z = gravity_params_.CoG_z; + force_value = gravity_params_.force; + gravity.vector.z = -force_value; + tf2::doTransform(gravity, gravity_transformed, p_tfBuffer->lookupTransform(sensor_frame_, transform_frame_, ros::Time(0))); + geometry_msgs::Wrench offset; + calibrate(false, &offset); + offset_.force.x -= gravity_transformed.vector.x; + offset_.force.y -= gravity_transformed.vector.y; + offset_.force.z -= gravity_transformed.vector.z; + offset_.torque.x -= (gravity_transformed.vector.y * cog.z - gravity_transformed.vector.z * cog.y); + offset_.torque.y -= (gravity_transformed.vector.z * cog.x - gravity_transformed.vector.x * cog.z); + offset_.torque.z -= (gravity_transformed.vector.x * cog.y - gravity_transformed.vector.y * cog.x); + res.success = true; + res.message = "Successfully recalibrated FTS!"; + return true; +} + +bool ForceTorqueSensorHWHandler::srvCallback_setSensorOffset(force_torque_sensor::SetSensorOffset::Request &req, force_torque_sensor::SetSensorOffset::Response &res) +{ + offset_.force.x = req.offset.force.x; + offset_.force.y = req.offset.force.y; + offset_.force.z = req.offset.force.z; + offset_.torque.x = req.offset.torque.x; + offset_.torque.y = req.offset.torque.y; + offset_.torque.z = req.offset.torque.z; + + res.success = true; + res.message = "Offset is successfully set!"; + return true; +} + + +bool ForceTorqueSensorHWHandler::calibrate(bool apply_after_calculation, geometry_msgs::Wrench *new_offset) +{ + apply_offset = false; + ROS_INFO("Calibrating using %d measurements and %f s pause between measurements.", calibrationNMeasurements, calibrationTBetween); + geometry_msgs::Wrench temp_offset = makeAverageMeasurement(calibrationNMeasurements, calibrationTBetween); + + apply_offset = true; + if (apply_after_calculation) { + offset_ = temp_offset; + } + + ROS_INFO("Calculated Calibration Offset: Fx: %f; Fy: %f; Fz: %f; Mx: %f; My: %f; Mz: %f", temp_offset.force.x, temp_offset.force.y, temp_offset.force.z, temp_offset.torque.x, temp_offset.torque.y, temp_offset.torque.z); + + m_isCalibrated = true; + *new_offset = temp_offset; + + return m_isCalibrated; +} + +geometry_msgs::Wrench ForceTorqueSensorHWHandler::makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id) +{ + geometry_msgs::Wrench measurement; + int num_of_errors = 0; + ros::Duration duration(time_between_meas); + for (int i = 0; i < number_of_measurements; i++) + { + geometry_msgs::Wrench output; + //std::cout<<"frame id"<< frame_id< 200){ + return measurement; + } + i--; + continue; + } + } + else + { + output = moving_mean_filtered_wrench.wrench; + } + measurement.force.x += output.force.x; + measurement.force.y += output.force.y; + measurement.force.z += output.force.z; + measurement.torque.x += output.torque.x; + measurement.torque.y += output.torque.y; + measurement.torque.z+= output.torque.z; + duration.sleep(); + } + measurement.force.x /= number_of_measurements; + measurement.force.y /= number_of_measurements; + measurement.force.z /= number_of_measurements; + measurement.torque.x /= number_of_measurements; + measurement.torque.y /= number_of_measurements; + measurement.torque.z /= number_of_measurements; + return measurement; +} + + +// TODO: make this to use filtered data (see calibrate) +bool ForceTorqueSensorHWHandler::srvCallback_DetermineCoordinateSystem(std_srvs::Trigger::Request& req, + std_srvs::Trigger::Response& res) +{ + if (m_isInitialized && m_isCalibrated) + { + double angle; + + ROS_INFO("Please push FTS with force larger than 10 N in desired direction of new axis %d", + coordinateSystemPushDirection); + + for (int i = 0; i < coordinateSystemNMeasurements; i++) + { + int status = 0; + double Fx, Fy, Fz, Tx, Ty, Tz = 0; + p_Ftc->readFTData(status, Fx, Fy, Fz, Tx, Ty, Tz); + + angle += atan2(Fy, Fx); + + usleep(coordinateSystemTBetween); + } + + angle /= coordinateSystemNMeasurements; + + if (coordinateSystemPushDirection) + { + angle -= M_PI / 2; + } + + ROS_INFO("Please rotate your coordinate system for %f rad (%f deg) around z-axis", angle, angle / M_PI * 180.0); + + res.success = true; + res.message = "CoordianteSystem successfull! :)"; + } + else + { + res.success = false; + res.message = "FTS not initialised or not calibrated! :/"; + } + + return true; +} + +bool ForceTorqueSensorHWHandler::srvReadDiagnosticVoltages(force_torque_sensor::DiagnosticVoltages::Request& req, + force_torque_sensor::DiagnosticVoltages::Response& res) +{ + p_Ftc->readDiagnosticADCVoltages(req.index, res.adc_value); + + return true; +} + +void ForceTorqueSensorHWHandler::pullFTData() +{ +// ros::Time timestamp = ros::Time::now(); + if (p_Ftc->readFTData(0, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z, + sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z) + != false) + { + sensor_data.header.stamp = ros::Time::now(); + sensor_data.header.frame_id = sensor_frame_; + if (apply_offset) { + sensor_data.wrench.force.x -= offset_.force.x; + sensor_data.wrench.force.y -= offset_.force.y; + sensor_data.wrench.force.z -= offset_.force.z; + sensor_data.wrench.torque.x -= offset_.torque.x; + sensor_data.wrench.torque.y -= offset_.torque.y; + sensor_data.wrench.torque.z -= offset_.torque.z; + } + + //lowpass + low_pass_filtered_data.header = sensor_data.header; + if(useLowPassFilter){ + low_pass_filter_->update(sensor_data,low_pass_filtered_data); + } + else low_pass_filtered_data = sensor_data; + + //moving_mean + moving_mean_filtered_wrench.header = low_pass_filtered_data.header; + if(useMovingMean){ + moving_mean_filter_->update(low_pass_filtered_data, moving_mean_filtered_wrench); + } + else moving_mean_filtered_wrench = low_pass_filtered_data; + + if(is_pub_sensor_data_) + if (sensor_data_pub_->trylock()){ + sensor_data_pub_->msg_ = sensor_data; + sensor_data_pub_->unlockAndPublish(); + } + + if(is_pub_low_pass_) + if (low_pass_pub_->trylock()){ + low_pass_pub_->msg_ = low_pass_filtered_data; + low_pass_pub_->unlockAndPublish(); + } + + if(is_pub_moving_mean_) + if (moving_mean_pub_->trylock()){ + moving_mean_pub_->msg_ = moving_mean_filtered_wrench; + moving_mean_pub_->unlockAndPublish(); + } + } + +// std::cout << (ros::Time::now() - timestamp).toNSec()/1000.0 << " ms" << std::endl; +} + +void ForceTorqueSensorHWHandler::filterFTData(){ + + transformed_data.header.stamp = moving_mean_filtered_wrench.header.stamp; + transformed_data.header.frame_id = transform_frame_; + if (transform_wrench(transform_frame_, sensor_frame_, moving_mean_filtered_wrench.wrench, transformed_data.wrench)) + { + //gravity compensation + if(useGravityCompensator) + { + gravity_compensator_->update(transformed_data, gravity_compensated_force); + } + else gravity_compensated_force = transformed_data; + + //treshhold filtering + if(useThresholdFilter) + { + threshold_filter_->update(gravity_compensated_force, threshold_filtered_force); + } + else threshold_filtered_force = gravity_compensated_force; + + if(is_pub_transformed_data_) + if (transformed_data_pub_->trylock()){ + transformed_data_pub_->msg_ = transformed_data; + transformed_data_pub_->unlockAndPublish(); + } + if(is_pub_gravity_compensated_ && useGravityCompensator) + if (gravity_compensated_pub_->trylock()){ + gravity_compensated_pub_->msg_ = gravity_compensated_force; + gravity_compensated_pub_->unlockAndPublish(); + } + + if(is_pub_threshold_filtered_ && useThresholdFilter) + if (threshold_filtered_pub_->trylock()){ + threshold_filtered_pub_->msg_ = threshold_filtered_force; + threshold_filtered_pub_->unlockAndPublish(); + } + } +} + +bool ForceTorqueSensorHWHandler::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench& transformed) +{ + geometry_msgs::TransformStamped transform; + + try + { + transform = p_tfBuffer->lookupTransform(goal_frame, source_frame, ros::Time(0)); + _num_transform_errors = 0; + } + catch (tf2::TransformException ex) + { + if (_num_transform_errors%100 == 0){ + ROS_ERROR("%s", ex.what()); + } + _num_transform_errors++; + return false; + } + + tf2::doTransform(wrench, transformed, transform); + + return true; +} + +void ForceTorqueSensorHWHandler::reconfigureCalibrationRequest(force_torque_sensor::CalibrationConfig& config, uint32_t level) +{ + calibration_params_.fromConfig(config); + + calibrationTBetween = calibration_params_.T_between_meas; + m_staticCalibration = calibration_params_.isStatic; +} + +void ForceTorqueSensorHWHandler::updateFTData() +{ + filterFTData(); + + interface_force_[0] = threshold_filtered_force.wrench.force.x; + interface_force_[1] = threshold_filtered_force.wrench.force.y; + interface_force_[2] = threshold_filtered_force.wrench.force.z; + + interface_torque_[0] = threshold_filtered_force.wrench.torque.x; + interface_torque_[1] = threshold_filtered_force.wrench.torque.y; + interface_torque_[2] = threshold_filtered_force.wrench.torque.z; +} + + PLUGINLIB_EXPORT_CLASS(force_torque_sensor::ForceTorqueSensorHWHandler, hardware_interface::SensorHW) diff --git a/src/force_torque_sensor_sim.cpp b/src/force_torque_sensor_sim.cpp index 7e9e92d..1b02828 100644 --- a/src/force_torque_sensor_sim.cpp +++ b/src/force_torque_sensor_sim.cpp @@ -48,4 +48,23 @@ bool ForceTorqueSensorSim::readDiagnosticADCVoltages(int index, short int& value std::cout<<"ForceTorqueSensorSim"< + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 006dc6b3c50f10166e197ecaeb835be9668245e2 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Fri, 1 Mar 2019 10:45:36 +0100 Subject: [PATCH 2/5] changed ifndef in sensor_hw_handler --- include/force_torque_sensor/force_torque_sensor_hw_handler.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/force_torque_sensor/force_torque_sensor_hw_handler.h b/include/force_torque_sensor/force_torque_sensor_hw_handler.h index f0add91..efa4341 100644 --- a/include/force_torque_sensor/force_torque_sensor_hw_handler.h +++ b/include/force_torque_sensor/force_torque_sensor_hw_handler.h @@ -41,8 +41,8 @@ * ****************************************************************/ -#ifndef FORCETORQUESENSORHANDLE_INCLUDEDEF_H -#define FORCETORQUESENSORHANDLE_INCLUDEDEF_H +#ifndef FORCETORQUESENSORHWHANDLER_INCLUDEDEF_H +#define FORCETORQUESENSORHWHANDLER_INCLUDEDEF_H #include #include From 45e152d8377785a2f6ff1797966d68602b11a7f3 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Tue, 5 Mar 2019 11:55:56 +0100 Subject: [PATCH 3/5] added two old constructors for kinetic in handler --- .../force_torque_sensor_hw_handler.h | 14 ++- src/force_torque_sensor_hw_handler.cpp | 88 ++++++++++++++++--- src/force_torque_sensor_node.cpp | 4 +- 3 files changed, 88 insertions(+), 18 deletions(-) diff --git a/include/force_torque_sensor/force_torque_sensor_hw_handler.h b/include/force_torque_sensor/force_torque_sensor_hw_handler.h index efa4341..fc8c08a 100644 --- a/include/force_torque_sensor/force_torque_sensor_hw_handler.h +++ b/include/force_torque_sensor/force_torque_sensor_hw_handler.h @@ -100,7 +100,9 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH { public: ForceTorqueSensorHWHandler(); - + ForceTorqueSensorHWHandler(ros::NodeHandle &nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame); + ForceTorqueSensorHWHandler(ros::NodeHandle &nh, std::string sensor_name, std::string output_frame); + void prepareNode(std::string output_frame); void init_sensor(std::string &msg, bool &success); @@ -118,7 +120,8 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH void read ( const ros::Time& time, const ros::Duration& period ); private: - void updateFTData(); + void updateFTData(const ros::TimerEvent &event); + void updateFTData_(); geometry_msgs::Wrench makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id=""); bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench& transformed); @@ -138,7 +141,8 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH std::string transform_frame_; std::string sensor_frame_; - void pullFTData(); + void pullFTData(const ros::TimerEvent &event); + void pullFTData_(); void filterFTData(); // Arrays for dumping FT-Data @@ -189,6 +193,7 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH ros::ServiceServer srvServer_SetSensorOffset; ros::Timer ftUpdateTimer_, ftPullTimer_; + bool using_timer; tf2_ros::TransformListener *p_tfListener; tf2::Transform transform_ee_base; @@ -224,6 +229,9 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH hardware_interface::ForceTorqueSensorInterface fts_interface_; ros::Time last_publish_time_; ros::Time last_pull_time_; + + void registerHandleAndInterface(std::string sensor_name, std::string output_frame); + bool loadSensor(std::string sensor_hw, std::string transform_frame); }; } diff --git a/src/force_torque_sensor_hw_handler.cpp b/src/force_torque_sensor_hw_handler.cpp index 02bc39c..8e55189 100644 --- a/src/force_torque_sensor_hw_handler.cpp +++ b/src/force_torque_sensor_hw_handler.cpp @@ -49,50 +49,85 @@ using namespace force_torque_sensor; ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler() : calibration_params_ {nh_.getNamespace() + "/Calibration/Offset"}, CS_params_ {nh_.getNamespace() }, HWComm_params_ {nh_.getNamespace() + "/HWComm"}, FTS_params_ {nh_.getNamespace() + "/FTS"}, pub_params_ {nh_.getNamespace() + "/Publish"}, node_params_ {nh_.getNamespace() + "/Node"}, gravity_params_ {nh_.getNamespace() + "/GravityCompensation/params"} {} -bool ForceTorqueSensorHWHandler::init(ros::NodeHandle &root_nh, ros::NodeHandle &sensor_hw_nh) +ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler(ros::NodeHandle& nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame) : + nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"} +{ + p_Ftc = sensor; + using_timer = true; + prepareNode(output_frame); + registerHandleAndInterface(sensor_name, output_frame); +} + +ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) : + nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"} { node_params_.fromParamServer(); + using_timer = true; + loadSensor(node_params_.sensor_hw, output_frame); + registerHandleAndInterface(sensor_name, output_frame); +} + +void ForceTorqueSensorHWHandler::registerHandleAndInterface(std::string sensor_name, std::string output_frame) +{ + hardware_interface::ForceTorqueSensorHandle fts_handle(sensor_name, output_frame, interface_force_, interface_torque_); + fts_interface_.registerHandle(fts_handle); + registerInterface(&fts_interface_); +} +bool ForceTorqueSensorHWHandler::loadSensor(std::string sensor_hw, std::string transform_frame) +{ sensor_loader_.reset(new pluginlib::ClassLoader ("force_torque_sensor", "hardware_interface::ForceTorqueSensorHW")); - if (!node_params_.sensor_hw.empty()) { + if (sensor_hw != "") { try { - sensor_.reset(sensor_loader_->createUnmanagedInstance(node_params_.sensor_hw)); - ROS_INFO_STREAM("Sensor type " << node_params_.sensor_hw << " was successfully loaded."); + sensor_.reset(sensor_loader_->createUnmanagedInstance(sensor_hw)); + ROS_INFO_STREAM("Sensor type " << sensor_hw << " was successfully loaded."); p_Ftc = sensor_.get(); - prepareNode(node_params_.transform_frame); + prepareNode(transform_frame); } catch (pluginlib::PluginlibException &e) { ROS_ERROR_STREAM("Plugin failed to load:" << e.what()); + return false; } } else { ROS_ERROR_STREAM("Failed to getParam 'sensor_hw' (namespace: " << nh_.getNamespace() << ")."); ROS_ERROR("Sensor hardware failed to load"); return false; } + return true; +} + +bool ForceTorqueSensorHWHandler::init(ros::NodeHandle &root_nh, ros::NodeHandle &sensor_hw_nh) +{ + using_timer = false; + + node_params_.fromParamServer(); + + if (!loadSensor(node_params_.sensor_hw, node_params_.transform_frame)) { + return false; + } last_publish_time_ = ros::Time::now(); last_pull_time_ = ros::Time::now(); - hardware_interface::ForceTorqueSensorHandle fts_handle(FTS_params_.fts_name, node_params_.sensor_frame, interface_force_, interface_torque_); - fts_interface_.registerHandle(fts_handle); - registerInterface(&fts_interface_); + registerHandleAndInterface(FTS_params_.fts_name, node_params_.sensor_frame); return true; } void ForceTorqueSensorHWHandler::read(const ros::Time &time, const ros::Duration &period) { + if (using_timer) { return; } if (nodePullFreq > 0.0 && last_pull_time_ + ros::Duration(1.0 / nodePullFreq) < time) { - pullFTData(); + pullFTData_(); } if (nodePubFreq > 0.0 && last_publish_time_ + ros::Duration(1.0 / nodePubFreq) < time) { - updateFTData(); + updateFTData_(); } } void ForceTorqueSensorHWHandler::prepareNode(std::string output_frame) -{ +{ ROS_INFO_STREAM ("Sensor is using namespace '" << nh_.getNamespace() << "'."); transform_frame_ = output_frame; @@ -186,6 +221,11 @@ void ForceTorqueSensorHWHandler::prepareNode(std::string output_frame) transformed_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "transformed_data", 1); } + if (using_timer) { + ftUpdateTimer_ = nh_.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensorHWHandler::updateFTData, this, false, false); + ftPullTimer_ = nh_.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensorHWHandler::pullFTData, this, false, false); + } + //Median Filter if(nh_.hasParam("MovingMeanFilter")) { useMovingMean = true; @@ -229,6 +269,10 @@ void ForceTorqueSensorHWHandler::init_sensor(std::string& msg, bool& success) // read return init status and check it! if (p_Ftc->init()) { + if (using_timer) { + ftPullTimer_.start(); + } + m_isInitialized = true; success = true; msg = "FTS initalised!"; @@ -267,6 +311,10 @@ void ForceTorqueSensorHWHandler::init_sensor(std::string& msg, bool& success) success = false; msg = "FTS could not be initilised! :/"; } + + if (using_timer) { + ftUpdateTimer_.start(); + } } } @@ -489,7 +537,14 @@ bool ForceTorqueSensorHWHandler::srvReadDiagnosticVoltages(force_torque_sensor:: return true; } -void ForceTorqueSensorHWHandler::pullFTData() +void ForceTorqueSensorHWHandler::pullFTData(const ros::TimerEvent &event) +{ + if (using_timer) { + pullFTData_(); + } +} + +void ForceTorqueSensorHWHandler::pullFTData_() { // ros::Time timestamp = ros::Time::now(); if (p_Ftc->readFTData(0, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z, @@ -613,7 +668,14 @@ void ForceTorqueSensorHWHandler::reconfigureCalibrationRequest(force_torque_sens m_staticCalibration = calibration_params_.isStatic; } -void ForceTorqueSensorHWHandler::updateFTData() +void ForceTorqueSensorHWHandler::updateFTData(const ros::TimerEvent& event) +{ + if (using_timer) { + updateFTData_(); + } +} + +void ForceTorqueSensorHWHandler::updateFTData_() { filterFTData(); diff --git a/src/force_torque_sensor_node.cpp b/src/force_torque_sensor_node.cpp index d39ba98..a854668 100644 --- a/src/force_torque_sensor_node.cpp +++ b/src/force_torque_sensor_node.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -16,7 +16,7 @@ int main(int argc, char **argv) node_params_.fromParamServer(); - new force_torque_sensor::ForceTorqueSensorHandle(nh, node_params_.sensor_frame,node_params_.transform_frame); + new force_torque_sensor::ForceTorqueSensorHWHandler(nh, node_params_.sensor_frame,node_params_.transform_frame); ROS_INFO("ForceTorque Sensor Node running."); From 45901923fc5f136f35bc1f6d13cc97324ff391f6 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Tue, 5 Mar 2019 12:14:11 +0100 Subject: [PATCH 4/5] removed handle --- CMakeLists.txt | 1 - .../force_torque_sensor_handle.h | 226 ------- src/force_torque_sensor_handle.cpp | 627 ------------------ 3 files changed, 854 deletions(-) delete mode 100644 include/force_torque_sensor/force_torque_sensor_handle.h delete mode 100644 src/force_torque_sensor_handle.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ca3d4d9..fa2ec9c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,7 +86,6 @@ include_directories( ) add_library(${PROJECT_NAME} - src/force_torque_sensor_handle.cpp src/force_torque_sensor_hw.cpp src/force_torque_sensor_hw_handler.cpp ) diff --git a/include/force_torque_sensor/force_torque_sensor_handle.h b/include/force_torque_sensor/force_torque_sensor_handle.h deleted file mode 100644 index 421774a..0000000 --- a/include/force_torque_sensor/force_torque_sensor_handle.h +++ /dev/null @@ -1,226 +0,0 @@ -/**************************************************************** - * - * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, - * Institute for Anthropomatics and Robotics (IAR) - - * Intelligent Process Control and Robotics (IPR), - * Karlsruhe Institute of Technology - * - * Maintainers: Denis Štogl, email: denis.stogl@kit.edu - * Florian Heller - * Vanessa Streuer - * - * Date of update: 2014-2016 - * - * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License LGPL as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License LGPL for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License LGPL along with this program. - * If not, see . - * - ****************************************************************/ - -#ifndef FORCETORQUESENSORHANDLE_INCLUDEDEF_H -#define FORCETORQUESENSORHANDLE_INCLUDEDEF_H - -#include -#include - -#include -typedef unsigned char uint8_t; -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#define PI 3.14159265 - -namespace force_torque_sensor -{ - -class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHandle -{ -public: - - ForceTorqueSensorHandle(ros::NodeHandle &nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame); - ForceTorqueSensorHandle(ros::NodeHandle &nh, std::string sensor_name, std::string output_frame); - - void prepareNode(std::string output_frame); - - void init_sensor(std::string &msg, bool &success); - bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); - bool srvCallback_CalculateOffset(force_torque_sensor::CalculateSensorOffset::Request &req, force_torque_sensor::CalculateSensorOffset::Response &res); - bool srvCallback_CalculateAverageMasurement(force_torque_sensor::CalculateAverageMasurement::Request &req, force_torque_sensor::CalculateAverageMasurement::Response &res); - bool calibrate(bool apply_after_calculation, geometry_msgs::Wrench *new_offset); - bool srvCallback_DetermineCoordinateSystem(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); - bool srvReadDiagnosticVoltages(force_torque_sensor::DiagnosticVoltages::Request &req, - force_torque_sensor::DiagnosticVoltages::Response &res); - bool srvCallback_recalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); - bool srvCallback_setSensorOffset(force_torque_sensor::SetSensorOffset::Request &req, - force_torque_sensor::SetSensorOffset::Response &res); - -private: - void updateFTData(const ros::TimerEvent &event); - geometry_msgs::Wrench makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id=""); - - bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench& transformed); - - // Arrays for hardware_interface - double interface_force_[3]; - double interface_torque_[3]; - - force_torque_sensor::CoordinateSystemCalibrationParameters CS_params_; - force_torque_sensor::HWCommunicationConfigurationParameters HWComm_params_; - force_torque_sensor::FTSConfigurationParameters FTS_params_; - force_torque_sensor::PublishConfigurationParameters pub_params_; - force_torque_sensor::NodeConfigurationParameters node_params_; - force_torque_sensor::CalibrationParameters calibration_params_; - iirob_filters::GravityCompensationParameters gravity_params_; - - std::string transform_frame_; - std::string sensor_frame_; - - void pullFTData(const ros::TimerEvent &event); - void filterFTData(); - - // Arrays for dumping FT-Data - geometry_msgs::WrenchStamped gravity_compensated_force, moving_mean_filtered_wrench, threshold_filtered_force, transformed_data, sensor_data, low_pass_filtered_data; - - double force_buffer_[3]; - double torque_buffer_[3]; - double force_buffer_transformed_[3]; - double torque_buffer_transformed_[3]; - - ros::NodeHandle nh_; - - //FT Data - hardware_interface::ForceTorqueSensorHW *p_Ftc; - geometry_msgs::Wrench offset_; - geometry_msgs::TransformStamped transform_ee_base_stamped; - tf2_ros::Buffer *p_tfBuffer; - realtime_tools::RealtimePublisher *gravity_compensated_pub_, *threshold_filtered_pub_, *transformed_data_pub_, *sensor_data_pub_, *low_pass_pub_, *moving_mean_pub_; - - bool is_pub_gravity_compensated_ = false; - bool is_pub_threshold_filtered_ = false; - bool is_pub_transformed_data_ = false; - bool is_pub_sensor_data_ = false; - bool is_pub_low_pass_ = false; - bool is_pub_moving_mean_ = false; - - uint _num_transform_errors; - - // HWComm parameters - int HWCommType; // Only important if can is used - std::string HWCommPath; - int HWCommBaudrate; - int ftsBaseID; - double nodePubFreq, nodePullFreq; - uint calibrationNMeasurements; - double calibrationTBetween; - int coordinateSystemNMeasurements; - int coordinateSystemTBetween; - int coordinateSystemPushDirection; - - // service servers - ros::ServiceServer srvServer_Init_; - ros::ServiceServer srvServer_CalculateAverageMasurement_; - ros::ServiceServer srvServer_CalculateOffset_; - ros::ServiceServer srvServer_DetermineCoordianteSystem_; - ros::ServiceServer srvServer_Temp_; - ros::ServiceServer srvServer_ReCalibrate; - ros::ServiceServer srvServer_SetSensorOffset; - - ros::Timer ftUpdateTimer_, ftPullTimer_; - - tf2_ros::TransformListener *p_tfListener; - tf2::Transform transform_ee_base; - - bool m_isInitialized; - bool m_isCalibrated; - bool apply_offset; - - // Variables for Static offset - bool m_staticCalibration; - geometry_msgs::Wrench m_calibOffset; - - - filters::FilterBase *moving_mean_filter_ = new iirob_filters::MovingMeanFilter(); - filters::FilterBase *low_pass_filter_ = new iirob_filters::LowPassFilter(); - filters::FilterBase *threshold_filter_ = new iirob_filters::ThresholdFilter(); - filters::FilterBase *gravity_compensator_ = new iirob_filters::GravityCompensator(); - - bool useGravityCompensator=false; - bool useThresholdFilter=false; - - bool useMovingMean = false; - bool useLowPassFilter = false; - - - dynamic_reconfigure::Server reconfigCalibrationSrv_; // Dynamic reconfiguration service - - void reconfigureCalibrationRequest(force_torque_sensor::CalibrationConfig& config, uint32_t level); - - boost::shared_ptr> sensor_loader_; - boost::shared_ptr sensor_; -}; - -} -#endif diff --git a/src/force_torque_sensor_handle.cpp b/src/force_torque_sensor_handle.cpp deleted file mode 100644 index 0d206df..0000000 --- a/src/force_torque_sensor_handle.cpp +++ /dev/null @@ -1,627 +0,0 @@ -/**************************************************************** - * - * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group, - * Institute for Anthropomatics and Robotics (IAR) - - * Intelligent Process Control and Robotics (IPR), - * Karlsruhe Institute of Technology - * - * Maintainers: Denis Štogl, email: denis.stogl@kit.edu - * Florian Heller - * Vanessa Streuer - * - * Date of update: 2014-2016 - * - * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License LGPL as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU Lesser General Public License LGPL for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License LGPL along with this program. - * If not, see . - * - ****************************************************************/ - -#include - -#include - -using namespace force_torque_sensor; - -ForceTorqueSensorHandle::ForceTorqueSensorHandle(ros::NodeHandle& nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame) : - hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"} -{ - p_Ftc = sensor; - prepareNode(output_frame); -} - -ForceTorqueSensorHandle::ForceTorqueSensorHandle(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) : - hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"} -{ - node_params_.fromParamServer(); - - //load specified sensor HW - sensor_loader_.reset (new pluginlib::ClassLoader("force_torque_sensor", "hardware_interface::ForceTorqueSensorHW")); - if (!node_params_.sensor_hw.empty()) - { - try - { - sensor_.reset (sensor_loader_->createUnmanagedInstance (node_params_.sensor_hw)); - ROS_INFO_STREAM ("Sensor type " << node_params_.sensor_hw << " was successfully loaded."); - - p_Ftc = sensor_.get(); - prepareNode(output_frame); - } - catch (pluginlib::PluginlibException& e) - { - ROS_ERROR_STREAM ("Plugin failed to load:" << e.what()); - } - } - else - { - ROS_ERROR_STREAM ("Failed to getParam 'sensor_hw' (namespace: " << nh.getNamespace() << ")."); - ROS_ERROR ("Sensor hardware failed to load"); - } -} - - -void ForceTorqueSensorHandle::prepareNode(std::string output_frame) -{ - ROS_INFO_STREAM ("Sensor is using namespace '" << nh_.getNamespace() << "'."); - - transform_frame_ = output_frame; - - reconfigCalibrationSrv_.setCallback(boost::bind(&ForceTorqueSensorHandle::reconfigureCalibrationRequest, this, _1, _2)); - - calibration_params_.fromParamServer(); - CS_params_.fromParamServer(); - HWComm_params_.fromParamServer(); - FTS_params_.fromParamServer(); - pub_params_.fromParamServer(); - node_params_.fromParamServer(); - gravity_params_.fromParamServer(); - - int calibNMeas; - calibNMeas=calibration_params_.n_measurements; - - if (calibNMeas <= 0) - { - ROS_WARN("Parameter 'Calibration/n_measurements' is %d (<=0) using default: 20", calibNMeas); - calibrationNMeasurements = 20; - } - else { - calibrationNMeasurements = (uint)calibNMeas; - } - calibrationTBetween = calibration_params_.T_between_meas; - m_staticCalibration = calibration_params_.isStatic; - - std::map forceVal,torqueVal; - forceVal = calibration_params_.force; - torqueVal = calibration_params_.torque; - - m_calibOffset.force.x = forceVal["x"]; - m_calibOffset.force.y = forceVal["y"]; - m_calibOffset.force.z = forceVal["z"]; - m_calibOffset.torque.x = torqueVal["x"]; - m_calibOffset.torque.y = torqueVal["y"]; - m_calibOffset.torque.z = torqueVal["z"]; - - bool isAutoInit = false; - m_isInitialized = false; - m_isCalibrated = false; - srvServer_Init_ = nh_.advertiseService("Init", &ForceTorqueSensorHandle::srvCallback_Init, this); - srvServer_CalculateAverageMasurement_ = nh_.advertiseService("CalculateAverageMasurement", &ForceTorqueSensorHandle::srvCallback_CalculateAverageMasurement, this); - srvServer_CalculateOffset_ = nh_.advertiseService("CalculateOffsets", &ForceTorqueSensorHandle::srvCallback_CalculateOffset, this); - srvServer_DetermineCoordianteSystem_ = nh_.advertiseService("DetermineCoordinateSystem", &ForceTorqueSensorHandle::srvCallback_DetermineCoordinateSystem, this); - srvServer_Temp_ = nh_.advertiseService("GetTemperature", &ForceTorqueSensorHandle::srvReadDiagnosticVoltages, this); - srvServer_ReCalibrate = nh_.advertiseService("Recalibrate", &ForceTorqueSensorHandle::srvCallback_recalibrate, this); - srvServer_SetSensorOffset = nh_.advertiseService("SetSensorOffset", &ForceTorqueSensorHandle::srvCallback_setSensorOffset, this); - - // Read data from parameter server - HWCommType = HWComm_params_.type; - HWCommPath = HWComm_params_.path; - HWCommBaudrate = HWComm_params_.baudrate; - ftsBaseID = FTS_params_.base_identifier; - isAutoInit = FTS_params_.auto_init; - nodePubFreq = node_params_.ft_pub_freq; - nodePullFreq = node_params_.ft_pull_freq; - sensor_frame_ = node_params_.sensor_frame; - - coordinateSystemNMeasurements = CS_params_.n_measurements; - coordinateSystemTBetween = CS_params_.T_between_meas; - coordinateSystemPushDirection = CS_params_.push_direction; - - p_tfBuffer = new tf2_ros::Buffer(); - p_tfListener = new tf2_ros::TransformListener(*p_tfBuffer, true); - - //Wrench Publisher - is_pub_gravity_compensated_ = pub_params_.gravity_compensated; - if(is_pub_gravity_compensated_){ - gravity_compensated_pub_ = new realtime_tools::RealtimePublisher(nh_, "gravity_compensated", 1); - } - is_pub_low_pass_ = pub_params_.low_pass; - if(is_pub_low_pass_){ - low_pass_pub_ = new realtime_tools::RealtimePublisher(nh_, "low_pass", 1); - } - is_pub_moving_mean_=pub_params_.moving_mean; - if(is_pub_moving_mean_){ - moving_mean_pub_ = new realtime_tools::RealtimePublisher(nh_, "moving_mean", 1); - } - is_pub_sensor_data_=pub_params_.sensor_data; - if(is_pub_sensor_data_){ - sensor_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "sensor_data", 1); - } - is_pub_threshold_filtered_ =pub_params_.threshold_filtered; - if(is_pub_threshold_filtered_){ - threshold_filtered_pub_ = new realtime_tools::RealtimePublisher(nh_, "threshold_filtered", 1); - } - is_pub_transformed_data_ = pub_params_.transformed_data; - if(is_pub_transformed_data_){ - transformed_data_pub_ = new realtime_tools::RealtimePublisher(nh_, "transformed_data", 1); - } - - ftUpdateTimer_ = nh_.createTimer(ros::Rate(nodePubFreq), &ForceTorqueSensorHandle::updateFTData, this, false, false); - ftPullTimer_ = nh_.createTimer(ros::Rate(nodePullFreq), &ForceTorqueSensorHandle::pullFTData, this, false, false); - - //Median Filter - if(nh_.hasParam("MovingMeanFilter")) { - useMovingMean = true; - moving_mean_filter_->configure(nh_.getNamespace()+"/MovingMeanFilter"); - } - - //Low Pass Filter - if(nh_.hasParam("LowPassFilter")) { - useLowPassFilter = true; - low_pass_filter_->configure(nh_.getNamespace()+"/LowPassFilter"); - } - - //Gravity Compenstation - if(nh_.hasParam("GravityCompensation")) { - useGravityCompensator = true; - gravity_compensator_->configure(nh_.getNamespace()+"/GravityCompensation"); - } - - //Threshold Filter - if(nh_.hasParam("ThresholdFilter")) { - useThresholdFilter = true; - threshold_filter_->configure(nh_.getNamespace()+"/ThresholdFilter"); - } - - p_Ftc->initCommunication(HWCommType, HWCommPath, HWCommBaudrate, ftsBaseID); - - if (isAutoInit) - { - std::string msg; - bool success; - ROS_INFO("Starting Autoinit..."); - init_sensor(msg, success); - ROS_INFO("Autoinit: %s", msg.c_str()); - } -} - -void ForceTorqueSensorHandle::init_sensor(std::string& msg, bool& success) -{ - if (!m_isInitialized) - { - // read return init status and check it! - if (p_Ftc->init()) - { - // start timer for reading FT-data - ftPullTimer_.start(); - - m_isInitialized = true; - success = true; - msg = "FTS initalised!"; - - // Calibrate sensor - if (m_staticCalibration) - { - ROS_INFO("Using static Calibration Offset from paramter server with parametes Force: x:%f, y:%f, z:%f; Torque: x: %f, y:%f, z:%f;", - m_calibOffset.force.x, m_calibOffset.force.y, m_calibOffset.force.z, - m_calibOffset.torque.x, m_calibOffset.torque.y, m_calibOffset.torque.z - ); - offset_.force.x = m_calibOffset.force.x; - offset_.force.y = m_calibOffset.force.y; - offset_.force.z = m_calibOffset.force.z; - offset_.torque.x= m_calibOffset.torque.x; - offset_.torque.y = m_calibOffset.torque.y; - offset_.torque.z = m_calibOffset.torque.z; - } - else - { - ROS_INFO("Calibrating sensor. Plase wait..."); - geometry_msgs::Wrench temp_offset; - if (not calibrate(true, &temp_offset)) - { - success = false; - msg = "Calibration failed! :/"; - } - } - - apply_offset = true; - - } - else - { - m_isInitialized = false; - success = false; - msg = "FTS could not be initilised! :/"; - } - ftUpdateTimer_.start(); - } -} - -bool ForceTorqueSensorHandle::srvCallback_Init(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) -{ - std::string msg; - bool success; - - init_sensor(msg, success); - res.message = msg; - res.success = success; - - return true; -} - -bool ForceTorqueSensorHandle::srvCallback_CalculateAverageMasurement(force_torque_sensor::CalculateAverageMasurement::Request& req, force_torque_sensor::CalculateAverageMasurement::Response& res) -{ - if (m_isInitialized) - { - res.success = true; - res.message = "Measurement successfull! :)"; - res.measurement = makeAverageMeasurement(req.N_measurements, req.T_between_meas, req.frame_id); - } - else - { - res.success = false; - res.message = "FTS not initialised! :/"; - } - - return true; -} - -bool ForceTorqueSensorHandle::srvCallback_CalculateOffset(force_torque_sensor::CalculateSensorOffset::Request& req, force_torque_sensor::CalculateSensorOffset::Response& res) -{ - if (m_isInitialized) - { - if (calibrate(req.apply_after_calculation, &res.offset)) - { - res.success = true; - res.message = "Calibration successfull! :)"; - } - else - { - res.success = false; - res.message = "Calibration failed! :/"; - } - } - else - { - res.success = false; - res.message = "FTS not initialised! :/"; - } - - return true; -} - -bool ForceTorqueSensorHandle::srvCallback_recalibrate(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) -{ - if (!m_isInitialized) - { - ROS_WARN("FTS-Node is not initialized, please initialize first!"); - res.success = false; - res.message = "Failed to recalibrate because Node is not initiliazed."; - return true; - } - if (!(nh_.hasParam("force") && nh_.hasParam("CoG_x") && nh_.hasParam("CoG_y") && nh_.hasParam("CoG_z"))) - { - ROS_ERROR("Cannot use dynamic recalibration without all values for Gravity Compensation, set parameters or use " - "'Calibrate' service instead."); - res.success = false; - res.message = "Failed to recalibrate because of missing Parameters for Gravity Compensation."; - return true; - } - geometry_msgs::Vector3Stamped gravity, gravity_transformed; - geometry_msgs::Vector3 cog; - double force_value; - cog.x = gravity_params_.CoG_x; - cog.y = gravity_params_.CoG_y; - cog.z = gravity_params_.CoG_z; - force_value = gravity_params_.force; - gravity.vector.z = -force_value; - tf2::doTransform(gravity, gravity_transformed, p_tfBuffer->lookupTransform(sensor_frame_, transform_frame_, ros::Time(0))); - geometry_msgs::Wrench offset; - calibrate(false, &offset); - offset_.force.x -= gravity_transformed.vector.x; - offset_.force.y -= gravity_transformed.vector.y; - offset_.force.z -= gravity_transformed.vector.z; - offset_.torque.x -= (gravity_transformed.vector.y * cog.z - gravity_transformed.vector.z * cog.y); - offset_.torque.y -= (gravity_transformed.vector.z * cog.x - gravity_transformed.vector.x * cog.z); - offset_.torque.z -= (gravity_transformed.vector.x * cog.y - gravity_transformed.vector.y * cog.x); - res.success = true; - res.message = "Successfully recalibrated FTS!"; - return true; -} - -bool ForceTorqueSensorHandle::srvCallback_setSensorOffset(force_torque_sensor::SetSensorOffset::Request &req, force_torque_sensor::SetSensorOffset::Response &res) -{ - offset_.force.x = req.offset.force.x; - offset_.force.y = req.offset.force.y; - offset_.force.z = req.offset.force.z; - offset_.torque.x = req.offset.torque.x; - offset_.torque.y = req.offset.torque.y; - offset_.torque.z = req.offset.torque.z; - - res.success = true; - res.message = "Offset is successfully set!"; - return true; -} - - -bool ForceTorqueSensorHandle::calibrate(bool apply_after_calculation, geometry_msgs::Wrench *new_offset) -{ - apply_offset = false; - ROS_INFO("Calibrating using %d measurements and %f s pause between measurements.", calibrationNMeasurements, calibrationTBetween); - geometry_msgs::Wrench temp_offset = makeAverageMeasurement(calibrationNMeasurements, calibrationTBetween); - - apply_offset = true; - if (apply_after_calculation) { - offset_ = temp_offset; - } - - ROS_INFO("Calculated Calibration Offset: Fx: %f; Fy: %f; Fz: %f; Mx: %f; My: %f; Mz: %f", temp_offset.force.x, temp_offset.force.y, temp_offset.force.z, temp_offset.torque.x, temp_offset.torque.y, temp_offset.torque.z); - - m_isCalibrated = true; - *new_offset = temp_offset; - - return m_isCalibrated; -} - -geometry_msgs::Wrench ForceTorqueSensorHandle::makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id) -{ - geometry_msgs::Wrench measurement; - int num_of_errors = 0; - ros::Duration duration(time_between_meas); - for (int i = 0; i < number_of_measurements; i++) - { - geometry_msgs::Wrench output; - //std::cout<<"frame id"<< frame_id< 200){ - return measurement; - } - i--; - continue; - } - } - else - { - output = moving_mean_filtered_wrench.wrench; - } - measurement.force.x += output.force.x; - measurement.force.y += output.force.y; - measurement.force.z += output.force.z; - measurement.torque.x += output.torque.x; - measurement.torque.y += output.torque.y; - measurement.torque.z+= output.torque.z; - duration.sleep(); - } - measurement.force.x /= number_of_measurements; - measurement.force.y /= number_of_measurements; - measurement.force.z /= number_of_measurements; - measurement.torque.x /= number_of_measurements; - measurement.torque.y /= number_of_measurements; - measurement.torque.z /= number_of_measurements; - return measurement; -} - - -// TODO: make this to use filtered data (see calibrate) -bool ForceTorqueSensorHandle::srvCallback_DetermineCoordinateSystem(std_srvs::Trigger::Request& req, - std_srvs::Trigger::Response& res) -{ - if (m_isInitialized && m_isCalibrated) - { - double angle; - - ROS_INFO("Please push FTS with force larger than 10 N in desired direction of new axis %d", - coordinateSystemPushDirection); - - for (int i = 0; i < coordinateSystemNMeasurements; i++) - { - int status = 0; - double Fx, Fy, Fz, Tx, Ty, Tz = 0; - p_Ftc->readFTData(status, Fx, Fy, Fz, Tx, Ty, Tz); - - angle += atan2(Fy, Fx); - - usleep(coordinateSystemTBetween); - } - - angle /= coordinateSystemNMeasurements; - - if (coordinateSystemPushDirection) - { - angle -= M_PI / 2; - } - - ROS_INFO("Please rotate your coordinate system for %f rad (%f deg) around z-axis", angle, angle / M_PI * 180.0); - - res.success = true; - res.message = "CoordianteSystem successfull! :)"; - } - else - { - res.success = false; - res.message = "FTS not initialised or not calibrated! :/"; - } - - return true; -} - -bool ForceTorqueSensorHandle::srvReadDiagnosticVoltages(force_torque_sensor::DiagnosticVoltages::Request& req, - force_torque_sensor::DiagnosticVoltages::Response& res) -{ - p_Ftc->readDiagnosticADCVoltages(req.index, res.adc_value); - - return true; -} - -void ForceTorqueSensorHandle::pullFTData(const ros::TimerEvent &event) -{ -// ros::Time timestamp = ros::Time::now(); - if (p_Ftc->readFTData(0, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z, - sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z) - != false) - { - sensor_data.header.stamp = ros::Time::now(); - sensor_data.header.frame_id = sensor_frame_; - if (apply_offset) { - sensor_data.wrench.force.x -= offset_.force.x; - sensor_data.wrench.force.y -= offset_.force.y; - sensor_data.wrench.force.z -= offset_.force.z; - sensor_data.wrench.torque.x -= offset_.torque.x; - sensor_data.wrench.torque.y -= offset_.torque.y; - sensor_data.wrench.torque.z -= offset_.torque.z; - } - - //lowpass - low_pass_filtered_data.header = sensor_data.header; - if(useLowPassFilter){ - low_pass_filter_->update(sensor_data,low_pass_filtered_data); - } - else low_pass_filtered_data = sensor_data; - - //moving_mean - moving_mean_filtered_wrench.header = low_pass_filtered_data.header; - if(useMovingMean){ - moving_mean_filter_->update(low_pass_filtered_data, moving_mean_filtered_wrench); - } - else moving_mean_filtered_wrench = low_pass_filtered_data; - - if(is_pub_sensor_data_) - if (sensor_data_pub_->trylock()){ - sensor_data_pub_->msg_ = sensor_data; - sensor_data_pub_->unlockAndPublish(); - } - - if(is_pub_low_pass_) - if (low_pass_pub_->trylock()){ - low_pass_pub_->msg_ = low_pass_filtered_data; - low_pass_pub_->unlockAndPublish(); - } - - if(is_pub_moving_mean_) - if (moving_mean_pub_->trylock()){ - moving_mean_pub_->msg_ = moving_mean_filtered_wrench; - moving_mean_pub_->unlockAndPublish(); - } - } - -// std::cout << (ros::Time::now() - timestamp).toNSec()/1000.0 << " ms" << std::endl; -} - -void ForceTorqueSensorHandle::filterFTData(){ - - transformed_data.header.stamp = moving_mean_filtered_wrench.header.stamp; - transformed_data.header.frame_id = transform_frame_; - if (transform_wrench(transform_frame_, sensor_frame_, moving_mean_filtered_wrench.wrench, transformed_data.wrench)) - { - //gravity compensation - if(useGravityCompensator) - { - gravity_compensator_->update(transformed_data, gravity_compensated_force); - } - else gravity_compensated_force = transformed_data; - - //treshhold filtering - if(useThresholdFilter) - { - threshold_filter_->update(gravity_compensated_force, threshold_filtered_force); - } - else threshold_filtered_force = gravity_compensated_force; - - if(is_pub_transformed_data_) - if (transformed_data_pub_->trylock()){ - transformed_data_pub_->msg_ = transformed_data; - transformed_data_pub_->unlockAndPublish(); - } - if(is_pub_gravity_compensated_ && useGravityCompensator) - if (gravity_compensated_pub_->trylock()){ - gravity_compensated_pub_->msg_ = gravity_compensated_force; - gravity_compensated_pub_->unlockAndPublish(); - } - - if(is_pub_threshold_filtered_ && useThresholdFilter) - if (threshold_filtered_pub_->trylock()){ - threshold_filtered_pub_->msg_ = threshold_filtered_force; - threshold_filtered_pub_->unlockAndPublish(); - } - } -} - -bool ForceTorqueSensorHandle::transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench& transformed) -{ - geometry_msgs::TransformStamped transform; - - try - { - transform = p_tfBuffer->lookupTransform(goal_frame, source_frame, ros::Time(0)); - _num_transform_errors = 0; - } - catch (tf2::TransformException ex) - { - if (_num_transform_errors%100 == 0){ - ROS_ERROR("%s", ex.what()); - } - _num_transform_errors++; - return false; - } - - tf2::doTransform(wrench, transformed, transform); - - return true; -} - -void ForceTorqueSensorHandle::reconfigureCalibrationRequest(force_torque_sensor::CalibrationConfig& config, uint32_t level) -{ - calibration_params_.fromConfig(config); - - calibrationTBetween = calibration_params_.T_between_meas; - m_staticCalibration = calibration_params_.isStatic; -} - -void ForceTorqueSensorHandle::updateFTData(const ros::TimerEvent& event) -{ - filterFTData(); - - interface_force_[0] = threshold_filtered_force.wrench.force.x; - interface_force_[1] = threshold_filtered_force.wrench.force.y; - interface_force_[2] = threshold_filtered_force.wrench.force.z; - - interface_torque_[0] = threshold_filtered_force.wrench.torque.x; - interface_torque_[1] = threshold_filtered_force.wrench.torque.y; - interface_torque_[2] = threshold_filtered_force.wrench.torque.z; -} From 49ba45d853950035c008bab5192abe34a8943d8f Mon Sep 17 00:00:00 2001 From: andreeatulbure Date: Thu, 7 Mar 2019 16:30:49 +0100 Subject: [PATCH 5/5] getFTSHandle for robot_layer_with_fts --- .../force_torque_sensor_hw_handler.h | 6 ++++-- src/force_torque_sensor_hw_handler.cpp | 13 ++++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/include/force_torque_sensor/force_torque_sensor_hw_handler.h b/include/force_torque_sensor/force_torque_sensor_hw_handler.h index fc8c08a..4722105 100644 --- a/include/force_torque_sensor/force_torque_sensor_hw_handler.h +++ b/include/force_torque_sensor/force_torque_sensor_hw_handler.h @@ -118,6 +118,7 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH force_torque_sensor::SetSensorOffset::Response &res); bool init ( ros::NodeHandle& root_nh, ros::NodeHandle &sensor_hw_nh ); void read ( const ros::Time& time, const ros::Duration& period ); + hardware_interface::ForceTorqueSensorHandle getFTSHandle(); private: void updateFTData(const ros::TimerEvent &event); @@ -225,11 +226,12 @@ class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorH boost::shared_ptr> sensor_loader_; boost::shared_ptr sensor_; - + + hardware_interface::ForceTorqueSensorHandle fts_handle; hardware_interface::ForceTorqueSensorInterface fts_interface_; ros::Time last_publish_time_; ros::Time last_pull_time_; - + void registerHandleAndInterface(std::string sensor_name, std::string output_frame); bool loadSensor(std::string sensor_hw, std::string transform_frame); }; diff --git a/src/force_torque_sensor_hw_handler.cpp b/src/force_torque_sensor_hw_handler.cpp index 8e55189..de17fa8 100644 --- a/src/force_torque_sensor_hw_handler.cpp +++ b/src/force_torque_sensor_hw_handler.cpp @@ -49,27 +49,30 @@ using namespace force_torque_sensor; ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler() : calibration_params_ {nh_.getNamespace() + "/Calibration/Offset"}, CS_params_ {nh_.getNamespace() }, HWComm_params_ {nh_.getNamespace() + "/HWComm"}, FTS_params_ {nh_.getNamespace() + "/FTS"}, pub_params_ {nh_.getNamespace() + "/Publish"}, node_params_ {nh_.getNamespace() + "/Node"}, gravity_params_ {nh_.getNamespace() + "/GravityCompensation/params"} {} -ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler(ros::NodeHandle& nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame) : +ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler(ros::NodeHandle& nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame) : fts_handle(hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_)), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"} { p_Ftc = sensor; using_timer = true; prepareNode(output_frame); - registerHandleAndInterface(sensor_name, output_frame); } -ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) : +ForceTorqueSensorHWHandler::ForceTorqueSensorHWHandler(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) : fts_handle(hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_)), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"} { node_params_.fromParamServer(); using_timer = true; loadSensor(node_params_.sensor_hw, output_frame); - registerHandleAndInterface(sensor_name, output_frame); +} + +hardware_interface::ForceTorqueSensorHandle ForceTorqueSensorHWHandler::getFTSHandle() +{ + return fts_handle; } void ForceTorqueSensorHWHandler::registerHandleAndInterface(std::string sensor_name, std::string output_frame) { - hardware_interface::ForceTorqueSensorHandle fts_handle(sensor_name, output_frame, interface_force_, interface_torque_); + fts_handle = hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_); fts_interface_.registerHandle(fts_handle); registerInterface(&fts_interface_); }