diff --git a/CMakeLists.txt b/CMakeLists.txt index ac17ca1..7c7fe99 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,30 +8,93 @@ find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs + controller_interface + controller_manager + hardware_interface + force_torque_sensor_controller + filters + gazebo_ros + #gazebo_plugins ) -catkin_package() +catkin_package( +INCLUDE_DIRS include + LIBRARIES force_torque_sensor_sim_ros_control + CATKIN_DEPENDS roscpp std_msgs hardware_interface controller_manager + DEPENDS system_lib) include_directories(include + include ${LIBXML2_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ) + add_library(FTSensor src/FTSensor.cpp - ) +) + +add_library(force_torque_sensor_sim_ros_control + src/force_torque_sensor_sim_ros_control.cpp +) + +add_library(force_torque_sensor_filter + src/trans_function_filter.cpp +) +add_library(imu_sensor_filter + src/imu_trans_function_filter.cpp +) + add_executable(ft_sensor_hw src/ft_sensor_hw.cpp ) +add_executable(force_torque_sensor_sim_ros_control_node src/force_torque_sensor_sim_ros_control.cpp) +add_executable(force_torque_sensor_filter_node src/trans_function_filter.cpp) +add_executable(imu_sensor_filter_node src/imu_trans_function_filter.cpp) + add_dependencies(ft_sensor_hw FTSensor ) +add_dependencies(force_torque_sensor_sim_ros_control_node + force_torque_sensor_sim_ros_control +) + +add_dependencies(force_torque_sensor_filter_node + force_torque_sensor_filter +) + +add_dependencies(imu_sensor_filter_node + imu_sensor_filter +) + target_link_libraries(ft_sensor_hw FTSensor ${LIBXML2_LIBRARIES} ${catkin_LIBRARIES} -) \ No newline at end of file +) + + + +target_link_libraries(force_torque_sensor_sim_ros_control_node + ${catkin_LIBRARIES} +) +target_link_libraries(force_torque_sensor_filter_node + ${catkin_LIBRARIES} +) +target_link_libraries(imu_sensor_filter_node + ${catkin_LIBRARIES} +) + + +add_dependencies(force_torque_sensor_sim_ros_control_node ${catkin_EXPORTED_TARGETS}) +add_dependencies(force_torque_sensor_filter_node ${catkin_EXPORTED_TARGETS}) +add_dependencies(imu_sensor_filter_node ${catkin_EXPORTED_TARGETS}) + + + +add_library(gazebo_ros_imu_sensor src/gazebo_ros_imu_sensor.cpp) +target_link_libraries(gazebo_ros_imu_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) \ No newline at end of file diff --git a/README.md b/README.md index 5ad405a..c93fc55 100755 --- a/README.md +++ b/README.md @@ -3,7 +3,31 @@ force-torque-sensor A catkin package to read measurements from any ATI FT sensor via XML -ToDO: - - [ ] Use force-torque-sensor controller from ros_control +Done: + - [x] Use force-torque-sensor controller from ros_control - [x] Create an URDF of the sensor to visualize the forces and frames - - [ ] Create plugin to be used in simulation + - [x] Create plugin to be used in simulation + - [x] Merge nano17 and gamma sensor + +Use in simulation: + +1) on first terminal for nano17 ft sensor type: + +roslaunch force_torque_sensor ft_simulation_display.launch use_gamma:=false + +or for gamma ft sensor type: + +roslaunch force_torque_sensor ft_simulation_display.launch + + +2) open a second terminal and type: + +rqt + +and add to rqt_plot the topic my_sensor/ft_sensor_topic/wrench/force my_sensor/ft_sensor_topic/wrench/torque + +3) open a third terminal and type + +roslaunch force_torque_sensor cylinder_spawner.launch + +A matlab esample of tuning the filter coefficient has been presented in matlab folder. \ No newline at end of file diff --git a/config/LowPass_Filter.yaml b/config/LowPass_Filter.yaml new file mode 100644 index 0000000..0895e7e --- /dev/null +++ b/config/LowPass_Filter.yaml @@ -0,0 +1,13 @@ +LowPass: + name: "LowPass" + type: "MultiChannelTransferFunctionFilterDouble" + params: + # param for PassbandFrequency 10 Hz + #a: [ 1.0, -1.6217, 0.68383] + #b: [ 0.015524, 0.031047, 0.015524] + # param for PassbandFrequency 5 Hz + a: [ 1.0, -1.8091, 0.82619] + b: [ 0.004266, 0.0085319, 0.004266] + # param for PassbandFrequency 1 Hz + #a: [1.0, -1.34, 0.50912] + #b: [0.042291, 0.084582, 0.042291] \ No newline at end of file diff --git a/config/force_torque_sensor_controller.yaml b/config/force_torque_sensor_controller.yaml new file mode 100644 index 0000000..03b5749 --- /dev/null +++ b/config/force_torque_sensor_controller.yaml @@ -0,0 +1,5 @@ +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 1000 + +# Settings for ros_control hardware interface diff --git a/config/ft_sensor_params.yaml b/config/ft_sensor_params.yaml new file mode 100644 index 0000000..b8e30f0 --- /dev/null +++ b/config/ft_sensor_params.yaml @@ -0,0 +1,6 @@ +force_torque_sensor_controller_params: + ft_sensor_name: "my_sensor" + ft_sensor_frame_id: "my_sensor_measure" + ft_sensor_topic: "/my_sensor/my_sensor/ft_sensor_topic" + + diff --git a/config/hardware_interface.yaml b/config/hardware_interface.yaml new file mode 100644 index 0000000..874b635 --- /dev/null +++ b/config/hardware_interface.yaml @@ -0,0 +1,12 @@ +# make the namespace match the name you use to instantiate the sensor in the xacro +#my_sensor: +#ForceTorqueSensorInterface: + #type: hardware_interface/ForceTorqueSensorInterface + #type: hardware_interface/ForceTorqueSensorHandle +#hardware_interface: +# type: hardware_interface/ForceTorqueSensorInterface + root: my_sensor_base + tip: my_sensor_measure + joints: + - my_sensor_measure_joint + diff --git a/config/imu_sensor_params.yaml b/config/imu_sensor_params.yaml new file mode 100644 index 0000000..ced8328 --- /dev/null +++ b/config/imu_sensor_params.yaml @@ -0,0 +1,4 @@ +imu_sensor_params: + imu_sensor_name: "my_sensor" + imu_sensor_frame_id: "my_sensor_imu_measure" + imu_sensor_topic: "/my_sensor/my_sensor/imu" \ No newline at end of file diff --git a/config/joint_names.yaml b/config/joint_names.yaml new file mode 100644 index 0000000..39a9507 --- /dev/null +++ b/config/joint_names.yaml @@ -0,0 +1,2 @@ +joints: + - my_sensor_measure_joint \ No newline at end of file diff --git a/ft_sensor.rviz b/ft_sensor.rviz new file mode 100644 index 0000000..082c824 --- /dev/null +++ b/ft_sensor.rviz @@ -0,0 +1,178 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /WrenchStamped1 + - /WrenchStamped1/Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 595 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + my_sensor_base: + Value: true + my_sensor_measure: + Value: true + world: + Value: true + Marker Scale: 0.02 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + my_sensor_base: + my_sensor_measure: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz/WrenchStamped + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: WrenchStamped + Topic: /my_sensor/sensor_measurements + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + my_sensor_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + my_sensor_measure: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: my_sensor_measure + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.102991 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.145398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.650398 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 876 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a8000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1535 + X: 55 + Y: 24 diff --git a/include/FTSensor/force_torque_sensor_filtering.h b/include/FTSensor/force_torque_sensor_filtering.h new file mode 100644 index 0000000..dc908d5 --- /dev/null +++ b/include/FTSensor/force_torque_sensor_filtering.h @@ -0,0 +1,126 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Luca Gemma + * Team Pacman, + * Università di Pisa, Centro Piaggio + * All rights reserved. + * + * 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 Technische Universität Darmstadt nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE + * COPYRIGHT OWNER 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. + * + *********************************************************************/ +#ifndef FORCE_TORQUE_SENSOR_FILTERING_HPP___ +#define FORCE_TORQUE_SENSOR_FILTERING_HPP___ + +#include +#include + +// #include "filters/filter_base.h" +// #include "filters/realtime_circular_buffer.h" + + +#include + +#include + +#include + +#include + + + +namespace Force_Torque_Sensor_Filtering +{ + + +class MultiChannelTransferFunctionFilter: public filters::MultiChannelTransferFunctionFilter +{ + public: + + void Force_Torque_Sensor_Read(std::string filtering_ft_sensor_params_name); + + void Force_Torque_Sensor_Filtering(); + + void init(std::string filtering_ft_sensor_params_name); + + bool config(); + + void cleanup(); + + void pub_Wrench_filtered(ros::Publisher publisher); + + //int number_of_channels_; + std::string ft_sensor_name; + + + private: + + void read(const geometry_msgs::WrenchStamped::ConstPtr &_ws); + + ros::NodeHandle nh_; + + //std::string ft_sensor_name; + std::string ft_sensor_frame_id; + std::string ft_sensor_topic; + //std::string filtering_ft_sensor_params_name; + + std::string filter_name; + std::string filter_type; + // std::string a_str; + // std::string b_str; + + // std::vector a_; //Transfer functon coefficients (output). + // std::vector b_; //Transfer functon coefficients (input). + + boost::shared_ptr subscriber_spinner_; + ros::CallbackQueue subscriber_queue_; + ros::Subscriber Sub_Wrench_; + ros::Publisher Pub_Wrench_; + + geometry_msgs::WrenchStamped WrenchStamped; + + // double force_[3]; + // double torque_[3]; + double data[6]; + //double data_in[6]; + // std::vector data_in; + // std::vector data_out; + + //filters::RealtimeCircularBuffer input_buffer_; + //filters::RealtimeCircularBuffer output_buffer_; + + + + geometry_msgs::WrenchStamped WrenchStamped_out; + +}; + +} + +#endif \ No newline at end of file diff --git a/include/FTSensor/force_torque_sensor_sim_ros_control.h b/include/FTSensor/force_torque_sensor_sim_ros_control.h new file mode 100644 index 0000000..a2acd0c --- /dev/null +++ b/include/FTSensor/force_torque_sensor_sim_ros_control.h @@ -0,0 +1,102 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Luca Gemma + * Team Pacman, + * Università di Pisa, Centro Piaggio + * All rights reserved. + * + * 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 Technische Universität Darmstadt nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE + * COPYRIGHT OWNER 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. + * + *********************************************************************/ + + +//#ifndef Force_Torque_Sensor_Sim_Ros_Control_hpp___ +//#define Force_Torque_Sensor_Sim_Ros_Control_hpp___ + +#ifndef FORCE_TORQUE_SENSOR_SIM_ROS_CONTROL_HPP___ +#define FORCE_TORQUE_SENSOR_SIM_ROS_CONTROL_HPP___ + +#include +#include +#include + +#include + +#include + +#include + + + +namespace force_torque_sensor_sim_ros_control +{ + +class ForceTorqueSensorSimRosControl : public hardware_interface::RobotHW +{ +public: + + ForceTorqueSensorSimRosControl(std::string ft_sensor_params_name); + + void init(std::string ft_sensor_params_name); + + void cleanup(); + + void read(ros::Time time, ros::Duration period); + + //std::string ft_topic_params; + +private: + + void force_torque_sensor_State(const geometry_msgs::WrenchStamped::ConstPtr &_ws); + + ros::NodeHandle nh_; + + std::string ft_sensor_name; + std::string ft_sensor_frame_id; + std::string ft_sensor_topic; + + boost::shared_ptr subscriber_spinner_; + ros::CallbackQueue subscriber_queue_; + ros::Subscriber Sub_Wrench_; + ros::Publisher Pub_Wrench_; + + hardware_interface::ForceTorqueSensorInterface force_torque_sensor_interface_; + + geometry_msgs::WrenchStamped WrenchStamped; + + double force_[3]; + double torque_[3]; + + +}; + +} + +#endif diff --git a/include/IMUSensor/gazebo_ros_imu_sensor.h b/include/IMUSensor/gazebo_ros_imu_sensor.h new file mode 100644 index 0000000..ea9ce7d --- /dev/null +++ b/include/IMUSensor/gazebo_ros_imu_sensor.h @@ -0,0 +1,107 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License.*/ + +#ifndef GAZEBO_ROS_IMU_SENSOR_H +#define GAZEBO_ROS_IMU_SENSOR_H + +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + namespace sensors + { + class ImuSensor; + } + /** + @anchor GazeboRosImuSensor + \ref GazeboRosImuSensor is a plugin to simulate an Inertial Motion Unit sensor, the main differences from \ref GazeboRosIMU are: + - inheritance from SensorPlugin instead of ModelPlugin, + - measurements are given by gazebo ImuSensor instead of being computed by the ros plugin, + - gravity is included in inertial measurements. + */ + /** @brief Gazebo Ros imu sensor plugin. */ + class GazeboRosImuSensor : public SensorPlugin + { + public: + /// \brief Constructor. + GazeboRosImuSensor(); + /// \brief Destructor. + virtual ~GazeboRosImuSensor(); + /// \brief Load the sensor. + /// \param sensor_ pointer to the sensor. + /// \param sdf_ pointer to the sdf config file. + virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_); + + protected: + /// \brief Update the sensor. + virtual void UpdateChild(const gazebo::common::UpdateInfo &/*_info*/); + + private: + /// \brief Load the parameters from the sdf file. + bool LoadParameters(); + /// \brief Gaussian noise generator. + /// \param mu offset value. + /// \param sigma scaling value. + double GuassianKernel(double mu, double sigma); + + /// \brief Ros NodeHandle pointer. + ros::NodeHandle* node; + /// \brief Ros Publisher for imu data. + ros::Publisher imu_data_publisher; + /// \brief Ros IMU message. + sensor_msgs::Imu imu_msg; + + /// \brief last time on which the data was published. + common::Time last_time; + /// \brief Pointer to the update event connection. + gazebo::event::ConnectionPtr connection; + /// \brief Pointer to the sensor. + sensors::ImuSensor* sensor; + /// \brief Pointer to the sdf config file. + sdf::ElementPtr sdf; + /// \brief Orientation data from the sensor. + gazebo::math::Quaternion orientation; + /// \brief Linear acceleration data from the sensor. + math::Vector3 accelerometer_data; + /// \brief Angular velocity data from the sensor. + math::Vector3 gyroscope_data; + + /// \brief Seed for the Gaussian noise generator. + unsigned int seed; + + //loaded parameters + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string robot_namespace; + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string topic_name; + /// \brief Name of the link of the IMU, unused parameter. + std::string body_name; + /// \brief Sensor update rate. + double update_rate; + /// \brief Gaussian noise. + double gaussian_noise; + /// \brief Offset parameter, position part is unused. + math::Pose offset; + }; +} + +#endif //GAZEBO_ROS_IMU_SENSOR_H \ No newline at end of file diff --git a/include/IMUSensor/imu_sensor_filtering.h b/include/IMUSensor/imu_sensor_filtering.h new file mode 100644 index 0000000..bd7b84c --- /dev/null +++ b/include/IMUSensor/imu_sensor_filtering.h @@ -0,0 +1,132 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Luca Gemma + * Team Pacman, + * Università di Pisa, Centro Piaggio + * All rights reserved. + * + * 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 Technische Universität Darmstadt nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE + * COPYRIGHT OWNER 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. + * + *********************************************************************/ +#ifndef IMU_SENSOR_FILTERING_HPP___ +#define IMU_SENSOR_FILTERING_HPP___ + +#include +#include + +// #include "filters/filter_base.h" +// #include "filters/realtime_circular_buffer.h" + + +#include + +#include + +#include + +#include + #include + +#define n_data 10 + + +namespace IMU_Sensor_Filtering +{ + + +class MultiChannelTransferFunctionFilter: public filters::MultiChannelTransferFunctionFilter +{ + public: + + void IMU_Sensor_Read(std::string filtering_imu_sensor_params_name); + + void IMU_Sensor_Filtering(); + + void init(std::string filtering_imu_sensor_params_name); + + bool config(); + + void cleanup(); + + void pub_IMU_filtered(ros::Publisher publisher); + + //int number_of_channels_; + std::string imu_sensor_name; + + + private: + + void read(const sensor_msgs::Imu::ConstPtr &imu_msg); + + ros::NodeHandle nh_; + + //std::string ft_sensor_name; + std::string imu_sensor_frame_id; + std::string imu_sensor_topic; + //std::string filtering_ft_sensor_params_name; + + std::string filter_name; + std::string filter_type; + // std::string a_str; + // std::string b_str; + + // std::vector a_; //Transfer functon coefficients (output). + // std::vector b_; //Transfer functon coefficients (input). + + boost::shared_ptr subscriber_spinner_; + ros::CallbackQueue subscriber_queue_; + + ros::Subscriber imu_data_subscriber; + /// \brief Ros Publisher for imu data. + ros::Publisher imu_data_publisher; + + /// \brief Ros IMU message. + sensor_msgs::Imu imu_msg_in; + + /// \brief Ros IMU message. + sensor_msgs::Imu imu_msg_out; + + + + // double force_[3]; + // double torque_[3]; + double data[n_data]; + //double data_in[6]; + // std::vector data_in; + // std::vector data_out; + + //filters::RealtimeCircularBuffer input_buffer_; + //filters::RealtimeCircularBuffer output_buffer_; + +}; + +} + +#endif \ No newline at end of file diff --git a/launch/cylinder_spawner.launch b/launch/cylinder_spawner.launch new file mode 100644 index 0000000..d55c357 --- /dev/null +++ b/launch/cylinder_spawner.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/launch/display.launch b/launch/display.launch index 4f4c4b7..37e1653 100644 --- a/launch/display.launch +++ b/launch/display.launch @@ -6,13 +6,52 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/display_simulation.launch b/launch/display_simulation.launch deleted file mode 100644 index ec430b2..0000000 --- a/launch/display_simulation.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/ft_sensor_hw.launch.xml b/launch/ft_sensor_hw.launch.xml index 028474c..52cf18a 100644 --- a/launch/ft_sensor_hw.launch.xml +++ b/launch/ft_sensor_hw.launch.xml @@ -1,18 +1,18 @@ - - - - + + - + - + + \ No newline at end of file diff --git a/launch/ft_sensor_params.yaml b/launch/ft_sensor_params.yaml new file mode 100644 index 0000000..f5d2d34 --- /dev/null +++ b/launch/ft_sensor_params.yaml @@ -0,0 +1,4 @@ +force_torque_sensor_controller: + ft_sensor_name: "my_sensor" + ft_sensor_frame_id: "my_sensor_measure" + ft_sensor_topic: "/my_sensor/ft_sensor_topic" diff --git a/launch/ft_simulation_display.launch b/launch/ft_simulation_display.launch new file mode 100644 index 0000000..9d58588 --- /dev/null +++ b/launch/ft_simulation_display.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/launch/gamma_display_simulation.launch b/launch/gamma_display_simulation.launch new file mode 100644 index 0000000..e14d1a1 --- /dev/null +++ b/launch/gamma_display_simulation.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/my_config_rviz.rviz b/launch/my_config_rviz.rviz new file mode 100644 index 0000000..8000c12 --- /dev/null +++ b/launch/my_config_rviz.rviz @@ -0,0 +1,205 @@ +Panels: + - Class: rviz/Displays + Help Height: 82 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /WrenchStamped1 + - /Imu2/Status1 + - /Imu2/Box properties1 + - /Imu2/Axes properties1 + - /Imu2/Acceleration properties1 + Splitter Ratio: 0.5 + Tree Height: 591 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + my_sensor_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + my_sensor_imu_measure: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + my_sensor_measure: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Arrow Width: 0.06 + Class: rviz/WrenchStamped + Enabled: true + Force Arrow Scale: 0.1 + Force Color: 255; 0; 0 + History Length: 1 + Name: WrenchStamped + Topic: /my_sensor/my_sensor/force_torque_sensor_filtered + Torque Arrow Scale: 0.1 + Torque Color: 0; 0; 127 + Value: true + - Alpha: 1 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: false + History Length: 1 + Name: Imu + Topic: /my_sensor/my_sensor/IMU_filtered + Value: false + - Acceleration properties: + Acc. vector alpha: 0.5 + Acc. vector color: 0; 85; 0 + Acc. vector scale: 0.05 + Derotate acceleration: true + Enable acceleration: true + Axes properties: + Axes scale: 0.05 + Enable axes: true + Box properties: + Box alpha: 0.5 + Box color: 0; 0; 255 + Enable box: true + x_scale: 0.05 + y_scale: 0.05 + z_scale: 0.05 + Class: rviz_imu_plugin/Imu + Enabled: true + Name: Imu + Topic: /my_sensor/my_sensor/IMU_filtered + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: my_sensor_measure + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.16632 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0105551 + Y: -0.00679958 + Z: -0.224134 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.5698 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.99359 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 876 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001d5000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000030f000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1535 + X: 65 + Y: 24 diff --git a/launch/nano17_display_simulation.launch b/launch/nano17_display_simulation.launch new file mode 100644 index 0000000..d42faa1 --- /dev/null +++ b/launch/nano17_display_simulation.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/matlab/createFit.m b/matlab/createFit.m new file mode 100644 index 0000000..d1c13ce --- /dev/null +++ b/matlab/createFit.m @@ -0,0 +1,38 @@ +function [fitresult, gof] = createFit1(close_vec, w) +%CREATEFIT1(CLOSE_VEC,W) +% Create a fit. +% +% Data for 'weight hand fitting' fit: +% X Input : close_vec +% Y Output: w +% Output: +% fitresult : a fit object representing the fit. +% gof : structure with goodness-of fit info. +% +% See also FIT, CFIT, SFIT. + +% Auto-generated by MATLAB on 15-Feb-2016 12:37:21 + + +%% Fit: 'weight hand fitting'. +[xData, yData] = prepareCurveData( close_vec, w ); + +% Set up fittype and options. +ft = fittype( 'smoothingspline' ); +opts = fitoptions( 'Method', 'SmoothingSpline' ); +opts.Normalize = 'on'; +opts.SmoothingParam = 0.999999993834772; + +% Fit model to data. +[fitresult, gof] = fit( xData, yData, ft, opts ); + +% Plot fit with data. +figure( 'Name', 'weight hand fitting' ); +h = plot( fitresult, xData, yData ); +legend( h, 'w vs. close_vec', 'weight hand fitting', 'Location', 'NorthEast' ); +% Label axes +xlabel close_vec +ylabel w +grid on + + diff --git a/matlab/filter_design.m b/matlab/filter_design.m new file mode 100644 index 0000000..9ae4c9b --- /dev/null +++ b/matlab/filter_design.m @@ -0,0 +1,76 @@ +%% Calcualte the coefficient (numerator and denominator [b, a]) of the filter for the force torque sensor filter. + +clear all +close all +clc + +%% Design the filter as a Low Pass Filter of a Second Order and Pass Band of 10 Hz +lpFilt = designfilt('lowpassiir','FilterOrder',2, ... + 'PassbandFrequency',10,'PassbandRipple',0.003, ... + 'SampleRate',1e3); +% show filter Filter Visualitazione Tool +fvtool(lpFilt) + +t = 0:.001:1; % time vector +x = ones(size(t)); % Create a step signal signal. +data_in = awgn(x,50,'measured'); % Add white Gaussian noise. + + +% Test filter on data simulated +dataOut = filter(lpFilt,data_in); + +% Returns filter parameters +[b,a] = tf(lpFilt); + +%Show filter parameters +disp('Coef a:');vpa(a,5) +disp('Coef b:');vpa(b,5) + +% Plot noise and filtered signal +figure(2) +plot(t,x,t,data_in,t,dataOut) +legend('Original signal','Signal with AWGN','Signal Filtered'); +grid on + +%% Design the filter as a Low Pass Filter of a Second Order and Pass Band of 10 Hz + +lpFilt = designfilt('lowpassiir','FilterOrder',2, ... + 'PassbandFrequency',5,'PassbandRipple',0.003, ... + 'SampleRate',1e3); +% show filter Filter Visualitazione Tool +fvtool(lpFilt) +% Test filter on data simulated +dataOut = filter(lpFilt,data_in); + +% Returns filter parameters +[b,a] = tf(lpFilt); +disp('Coef a:');vpa(a,5) +disp('Coef b:');vpa(b,5) + +% Plot noise and filtered signal +figure(4) +plot(t,x,t,data_in,t,dataOut) % Plot both signals. +legend('Original signal','Signal with AWGN','Signal Filtered'); +grid on + + +%% Design the filter as a Low Pass Filter of a Second Order and Pass Band of 1 Hz + +lpFilt = designfilt('lowpassiir','FilterOrder',4, ... + 'PassbandFrequency',1,'PassbandRipple',0.3, ... + 'SampleRate',1e3); +% show filter Filter Visualitazione Tool +fvtool(lpFilt) +% Test filter on data simulated +dataOut = filter(lpFilt,data_in); + +% Returns filter parameters +[b,a] = tf(lpFilt); +disp('Coef a:');vpa(a,5) +disp('Coef b:');vpa(b,5) + +% Plot noise and filtered signal +figure(4) +plot(t,x,t,data_in,t,dataOut) % Plot both signals. +legend('Original signal','Signal with AWGN','Signal Filtered'); +grid on \ No newline at end of file diff --git a/matlab/hand_weight_fitting.m b/matlab/hand_weight_fitting.m new file mode 100644 index 0000000..1e92498 --- /dev/null +++ b/matlab/hand_weight_fitting.m @@ -0,0 +1,18 @@ +clear all +close all +clc +close_vec=[0, 0.15, 0.25, 0.56, 0.7, 0.8, 0.9, 0.99]; +w=[6.98114, 6.80429, 6.75198, 6.97346, 6.86624, 6.80824, 6.62334, 6.67071]; + +[fitresult, gof] = createFit(close_vec, w) + +breaks=fitresult.p.breaks; +coeff=fitresult.p.coefs; + +for i=0:0.01:1 +% 0.0440 +% 0.4234 +% 0.6943 +% 0.9653 + +end \ No newline at end of file diff --git a/model/ft_sensor.gazebo.xacro b/model/ft_sensor.gazebo.xacro index 52f81f8..d3c7116 100644 --- a/model/ft_sensor.gazebo.xacro +++ b/model/ft_sensor.gazebo.xacro @@ -2,33 +2,94 @@ + + + - - true - - - true + + false + - + true + + + + + + + + + Gazebo/Blue + + 1 + 1000 + 1 + __default_topic__ + + + ${name}/imu + ${name} + ${name}_imu_measure + 1000.0 + + 0.003 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + + 0 0 0 0 0 0 + + + + + + + + false + + true + + + + + - + + 1000.0 - ${parent}_${name}/ft_sensor_topic - ${parent}_${name}_measure_joint + ${name}/ft_sensor_topic + ${name}_measure_joint gaussian 0.0 - 0.003 + 0.003 + + + + + + + + + + diff --git a/model/ft_sensor.trasmission.xacro b/model/ft_sensor.trasmission.xacro new file mode 100644 index 0000000..20d31b4 --- /dev/null +++ b/model/ft_sensor.trasmission.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + /${name}/ + + + + + + + + + + + + diff --git a/model/ft_sensor.urdf.xacro b/model/ft_sensor.urdf.xacro index 10c5b57..e73780b 100644 --- a/model/ft_sensor.urdf.xacro +++ b/model/ft_sensor.urdf.xacro @@ -1,37 +1,46 @@ + + - + - + - - + + + - - - - - + + + + + + + - - + + + + + + \ No newline at end of file diff --git a/model/ft_sensor_gamma.urdf.xacro b/model/ft_sensor_gamma.urdf.xacro new file mode 100644 index 0000000..5252bde --- /dev/null +++ b/model/ft_sensor_gamma.urdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/model/meshes/gamma.stl b/model/meshes/gamma.stl new file mode 100644 index 0000000..107849a Binary files /dev/null and b/model/meshes/gamma.stl differ diff --git a/model/models.urdf.xacro b/model/models.urdf.xacro index 6132916..564b979 100644 --- a/model/models.urdf.xacro +++ b/model/models.urdf.xacro @@ -11,10 +11,16 @@ + + + @@ -44,9 +50,9 @@ - + - + @@ -66,6 +72,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/package.xml b/package.xml index b11e090..e95aa22 100644 --- a/package.xml +++ b/package.xml @@ -14,11 +14,19 @@ message_generation std_msgs geometry_msgs + hardware_interface + controller_interface + controller_manager + force_torque_sensor_controller roscpp message_generation std_msgs geometry_msgs + hardware_interface + controller_interface + controller_manager + force_torque_sensor_controller diff --git a/robot/cylinder.urdf.xacro b/robot/cylinder.urdf.xacro new file mode 100644 index 0000000..b009275 --- /dev/null +++ b/robot/cylinder.urdf.xacro @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 100 + true + 1 + 1 + 100000000 + 1000.0 + + + \ No newline at end of file diff --git a/robot/gamma.urdf.xacro b/robot/gamma.urdf.xacro new file mode 100644 index 0000000..a02f1e1 --- /dev/null +++ b/robot/gamma.urdf.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/nano17.urdf.xacro b/robot/nano17.urdf.xacro index 63698d2..c9d6884 100644 --- a/robot/nano17.urdf.xacro +++ b/robot/nano17.urdf.xacro @@ -1,13 +1,13 @@ - + - + - + \ No newline at end of file diff --git a/src/force_torque_sensor_sim_ros_control.cpp b/src/force_torque_sensor_sim_ros_control.cpp new file mode 100644 index 0000000..b2fbc22 --- /dev/null +++ b/src/force_torque_sensor_sim_ros_control.cpp @@ -0,0 +1,191 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Luca Gemma + * Team Pacman, + * Università di Pisa, Centro Piaggio + * All rights reserved. + * + * 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 Technische Universität Darmstadt nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE + * COPYRIGHT OWNER 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. + * + *********************************************************************/ + + + +#include + +#include +#include + + +namespace force_torque_sensor_sim_ros_control +{ + void ForceTorqueSensorSimRosControl::init(std::string ft_sensor_params_name) + { + //ROS_INFO_STREAM("ft_sensor_params_name: "<< ft_sensor_params_name); + nh_.getParam(ft_sensor_params_name+"/ft_sensor_name", ft_sensor_name); + nh_.getParam(ft_sensor_params_name+"/ft_sensor_frame_id", ft_sensor_frame_id); + + ROS_INFO_STREAM("name: "<< ft_sensor_name); + ROS_INFO_STREAM("ft_sensor_frame_id: "<< ft_sensor_frame_id); + + + + hardware_interface::ForceTorqueSensorHandle force_torque_handle(ft_sensor_name, ft_sensor_frame_id, force_, torque_); + + force_torque_sensor_interface_.registerHandle(force_torque_handle); + + registerInterface(&force_torque_sensor_interface_); + } + + void ForceTorqueSensorSimRosControl::force_torque_sensor_State(const geometry_msgs::WrenchStamped::ConstPtr &_ws) + { + + WrenchStamped = *_ws; + + force_[0] = WrenchStamped.wrench.force.x; + force_[1] = WrenchStamped.wrench.force.y; + force_[2] = WrenchStamped.wrench.force.z; + torque_[0] = WrenchStamped.wrench.torque.x; + torque_[1] = WrenchStamped.wrench.torque.y; + torque_[2] = WrenchStamped.wrench.torque.z; + + //ROS_INFO("force %f , %f , %f ",(float)force_[0],(float)force_[1],(float)force_[2]); + //ROS_INFO("torque %f , %f , %f ",(float)torque_[0],(float)torque_[1],(float)torque_[2]); + + } + + ForceTorqueSensorSimRosControl::ForceTorqueSensorSimRosControl(std::string ft_sensor_params_name) + { + ros::NodeHandle* rosnode = new ros::NodeHandle(); + + rosnode->setCallbackQueue(&subscriber_queue_); + + ros::Time last_ros_time_; + bool wait = true; + while (wait) + { + last_ros_time_ = ros::Time::now(); + if (last_ros_time_.toSec() > 0) + wait = false; + } + nh_.getParam(ft_sensor_params_name+"/ft_sensor_topic", ft_sensor_topic); + ROS_INFO_STREAM("ft_sensor_topic: "<< ft_sensor_topic); + // ros topic subscribtions + ros::SubscribeOptions Sub_Wrench = + ros::SubscribeOptions::create( + ft_sensor_topic, 1,boost::bind(&ForceTorqueSensorSimRosControl::force_torque_sensor_State, this, _1), + ros::VoidPtr(), rosnode->getCallbackQueue()); + + Sub_Wrench_ = rosnode->subscribe(Sub_Wrench); + + subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_)); + subscriber_spinner_->start(); + + } + + + void ForceTorqueSensorSimRosControl::cleanup() + { + subscriber_spinner_->stop(); + } + + void ForceTorqueSensorSimRosControl::read(ros::Time time, ros::Duration period) + { + //Normally we would read joint angles from hardware here, but we get those via ROS topic as we use Gazebo + } + +} + +int main(int argc, char** argv){ + + try{ + ROS_INFO("starting"); + ros::init(argc, argv, "force_torque_sensor_sim_ros_control"); + + + + //sleep(2); + + // Publish sensor user mode + + ros::NodeHandle nh; + std::string ft_sensor_params_name; + + nh.getParam("ft_sensor_params_name", ft_sensor_params_name); + ROS_INFO_STREAM("ft_sensor_params_name: "<< ft_sensor_params_name); + + force_torque_sensor_sim_ros_control::ForceTorqueSensorSimRosControl force_torque_sensor_sim_ros_control_interface(ft_sensor_params_name); + + force_torque_sensor_sim_ros_control_interface.init(ft_sensor_params_name); + + ros::NodeHandle controller_nh("force_torque_sensor_controller"); + + ros::Publisher pub_user_mode_ = nh.advertise("ft_sensor/control_mode",1,true); + + std_msgs::String msg; + std::stringstream ss; + ss << "User"; + msg.data = ss.str(); + pub_user_mode_.publish(msg); + + controller_manager::ControllerManager cm(&force_torque_sensor_sim_ros_control_interface, controller_nh); + + ros::AsyncSpinner spinner(2); + spinner.start(); + + ros::Rate loop_rate(1000); + + ros::Time last_time = ros::Time::now(); + + while (ros::ok()) + { + + + ros::Time current_time = ros::Time::now(); + ros::Duration elapsed_time = current_time - last_time; + last_time = current_time; + + force_torque_sensor_sim_ros_control_interface.read(current_time, elapsed_time); + + cm.update(current_time, elapsed_time); + + loop_rate.sleep(); + } + + force_torque_sensor_sim_ros_control_interface.cleanup(); + } + catch(...) + { + ROS_ERROR("Unhandled exception!"); + return -1; + } + + return 0; +} diff --git a/src/ft_sensor_hw.cpp b/src/ft_sensor_hw.cpp index 5271940..5474217 100644 --- a/src/ft_sensor_hw.cpp +++ b/src/ft_sensor_hw.cpp @@ -43,7 +43,7 @@ class FTSensorHW priv_nh_.param("name", name_, "my_sensor"); priv_nh_.param("type", type_, "nano17"); - priv_nh_.param("ip", ip_, "192.168.0.100"); + priv_nh_.param("ip", ip_, "192.168.1.250"); char* ip = new char[ip_.size() + 1]; std::copy(ip_.begin(), ip_.end(), ip); @@ -66,8 +66,8 @@ class FTSensorHW // Set bias ftsensor_->setBias(); - // Advertise topic where readings are published - pub_sensor_readings_ = nh_.advertise(nh_.resolveName("sensor_measurements"), 10); + // Advertise topic where readings are published name_+ + pub_sensor_readings_ = nh_.advertise(nh_.resolveName(name_+"/ft_sensor_topic"), 10); // Advertise service for setting the bias srv_set_bias_ = nh_.advertiseService(nh_.resolveName("tare"), &FTSensorHW::setBiasCallback, this); @@ -102,8 +102,8 @@ void FTSensorHW::publishMeasurements() ftreadings.wrench.torque.z = measurements[5]; ftreadings.header.stamp = ros::Time::now(); - ftreadings.header.frame_id = name_ + "_" + type_ + "_" + "measure"; - + //ftreadings.header.frame_id = name_ + "_" + type_ + "_" + "measure"; + ftreadings.header.frame_id = name_ + "_" + "measure"; pub_sensor_readings_.publish(ftreadings); } diff --git a/src/gazebo_ros_imu_sensor.cpp b/src/gazebo_ros_imu_sensor.cpp new file mode 100644 index 0000000..3c2c1e3 --- /dev/null +++ b/src/gazebo_ros_imu_sensor.cpp @@ -0,0 +1,227 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License.*/ + +#include +#include +#include +#include + +GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboRosImuSensor) + +gazebo::GazeboRosImuSensor::GazeboRosImuSensor(): SensorPlugin() +{ + accelerometer_data = math::Vector3(0, 0, 0); + gyroscope_data = math::Vector3(0, 0, 0); + orientation = math::Quaternion(1,0,0,0); + seed=0; +} + +void gazebo::GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) +{ + sdf=sdf_; + sensor=dynamic_cast(sensor_.get()); + sensor->SetActive(true); + + if(!LoadParameters()) + { + ROS_FATAL("Error Loading Parameters!"); + return; + } + + if (!ros::isInitialized()) //check if ros is initialized properly + { + ROS_FATAL("ROS has not been initialized!"); + return; + } + + node = new ros::NodeHandle(this->robot_namespace); + + imu_data_publisher = node->advertise(topic_name,1); + + connection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosImuSensor::UpdateChild, this, _1)); + + last_time = sensor->GetLastUpdateTime(); +} + +void gazebo::GazeboRosImuSensor::UpdateChild(const gazebo::common::UpdateInfo &/*_info*/) +{ + common::Time current_time = sensor->GetLastUpdateTime(); + + if(update_rate>0 && (current_time-last_time).Double() < 1.0/update_rate) //update rate check + return; + + if(imu_data_publisher.getNumSubscribers() > 0) + { + orientation = offset.rot*sensor->GetOrientation(); //applying offsets to the orientation measurement + accelerometer_data = sensor->GetLinearAcceleration(); + gyroscope_data = sensor->GetAngularVelocity(); + imu_msg.header.stamp.sec = current_time.sec; + imu_msg.header.stamp.nsec = current_time.nsec; + + imu_msg.header.frame_id=body_name; + //Guassian noise is applied to all measurements + imu_msg.orientation.x = orientation.x + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.y = orientation.y + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.z = orientation.z + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.w = orientation.w + GuassianKernel(0,gaussian_noise); + + imu_msg.linear_acceleration.x = accelerometer_data.x + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.y = accelerometer_data.y + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.z = accelerometer_data.z + GuassianKernel(0,gaussian_noise); + + imu_msg.angular_velocity.x = gyroscope_data.x + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.y = gyroscope_data.y + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.z = gyroscope_data.z + GuassianKernel(0,gaussian_noise); + + //covariance is related to the Gaussian noise + double gn2 = gaussian_noise*gaussian_noise; + imu_msg.orientation_covariance[0] = gn2; + imu_msg.orientation_covariance[4] = gn2; + imu_msg.orientation_covariance[8] = gn2; + imu_msg.angular_velocity_covariance[0] = gn2; + imu_msg.angular_velocity_covariance[4] = gn2; + imu_msg.angular_velocity_covariance[8] = gn2; + imu_msg.linear_acceleration_covariance[0] = gn2; + imu_msg.linear_acceleration_covariance[4] = gn2; + imu_msg.linear_acceleration_covariance[8] = gn2; + + //publishing data + imu_data_publisher.publish(imu_msg); + + ros::spinOnce(); + } + + last_time = current_time; +} + +double gazebo::GazeboRosImuSensor::GuassianKernel(double mu, double sigma) +{ + // generation of two normalized uniform random variables + double U1 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + double U2 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + + // using Box-Muller transform to obtain a varaible with a standard normal distribution + double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2); + + // scaling + Z0 = sigma * Z0 + mu; + return Z0; +} + +bool gazebo::GazeboRosImuSensor::LoadParameters() +{ + //loading parameters from the sdf file + + //NAMESPACE + if (sdf->HasElement("robotNamespace")) + { + robot_namespace = "/" + sdf->Get("robotNamespace") +"/"; + ROS_INFO_STREAM(" set to: "<GetParentName(); //HACK to find the model name + std::size_t it = scoped_name.find("::"); + + robot_namespace = "/" +scoped_name.substr(0,it)+"/"; + ROS_WARN_STREAM("missing , set to default: " << robot_namespace); + } + + //TOPIC + if (sdf->HasElement("topicName")) + { + topic_name = robot_namespace + sdf->Get("topicName"); + ROS_INFO_STREAM(" set to: "<, set to /namespace/default: " << topic_name); + } + + //BODY, UNUSED + if (sdf->HasElement("bodyName")) + { + body_name = sdf->Get("bodyName"); + ROS_INFO_STREAM(" set to: "<, cannot proceed"); + return false; + } + + //UPDATE RATE + if (sdf->HasElement("updateRateHZ")) + { + update_rate = sdf->Get("updateRateHZ"); + ROS_INFO_STREAM(" set to: " << update_rate); + } + else + { + update_rate = 1.0; + ROS_WARN_STREAM("missing , set to default: " << update_rate); + } + + //NOISE + if (sdf->HasElement("gaussianNoise")) + { + gaussian_noise = sdf->Get("gaussianNoise"); + ROS_INFO_STREAM(" set to: " << gaussian_noise); + } + else + { + gaussian_noise = 0.0; + ROS_WARN_STREAM("missing , set to default: " << gaussian_noise); + } + + //POSITION OFFSET, UNUSED + if (sdf->HasElement("xyzOffset")) + { + offset.pos = sdf->Get("xyzOffset"); + ROS_INFO_STREAM(" set to: " << offset.pos[0] << ' ' << offset.pos[1] << ' ' << offset.pos[2]); + } + else + { + offset.pos = math::Vector3(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.pos[0] << ' ' << offset.pos[1] << ' ' << offset.pos[2]); + } + + //ORIENTATION OFFSET + if (sdf->HasElement("rpyOffset")) + { + offset.rot = sdf->Get("rpyOffset"); + ROS_INFO_STREAM(" set to: " << offset.rot.GetRoll() << ' ' << offset.rot.GetPitch() << ' ' << offset.rot.GetYaw()); + } + else + { + offset.rot = math::Vector3(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.rot.GetRoll() << ' ' << offset.rot.GetPitch() << ' ' << offset.rot.GetYaw()); + } + + return true; +} + +gazebo::GazeboRosImuSensor::~GazeboRosImuSensor() +{ + if (connection.get()) + { + gazebo::event::Events::DisconnectWorldUpdateBegin(connection); + connection = gazebo::event::ConnectionPtr(); + } + + node->shutdown(); +} \ No newline at end of file diff --git a/src/imu_trans_function_filter.cpp b/src/imu_trans_function_filter.cpp new file mode 100644 index 0000000..bc6da24 --- /dev/null +++ b/src/imu_trans_function_filter.cpp @@ -0,0 +1,293 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Luca Gemma + * Team Pacman, + * Università di Pisa, Centro Piaggio + * All rights reserved. + * + * 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 Technische Universität Darmstadt nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE + * COPYRIGHT OWNER 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. + * + *********************************************************************/ +#include +#include +#include + + +namespace IMU_Sensor_Filtering +{ + + void MultiChannelTransferFunctionFilter::init(std::string filtering_imu_sensor_params_name) + { + nh_.getParam(filtering_imu_sensor_params_name+"/imu_sensor_name", imu_sensor_name); + nh_.getParam(filtering_imu_sensor_params_name+"/imu_sensor_frame_id",imu_sensor_frame_id); + ROS_INFO_STREAM("filtering_name: "<< imu_sensor_name); + ROS_INFO_STREAM("filtering_ft_sensor_frame_id: "<< imu_sensor_frame_id); + + nh_.getParam("LowPass/name", filter_name); + ROS_INFO_STREAM(" IMU filter_name: "<< filter_name); + nh_.getParam("LowPass/type", filter_type); + ROS_INFO_STREAM("IMU filter_type: "<< filter_type); + + this->filter_type_=filter_name; + this->filter_type_=filter_type; + this->number_of_channels_=n_data; + + } + + bool MultiChannelTransferFunctionFilter::config() +{ + // Parse a and b into a std::vector. + + if (!nh_.getParam("LowPass/params/a", a_)) + { + ROS_ERROR("IMU TransferFunctionFilter, \"%s\", params has no attribute a.", filter_name.c_str()); + return false; + } + else{ + //ROS_INFO("config_filter_a : %f ,%f",(float)a_[0],(float)a_[1]); + ROS_INFO("Loaded a"); + } + + if (!nh_.getParam("LowPass/params/b", b_)) + { + ROS_ERROR("IMU TransferFunctionFilter, \"%s\", params has no attribute b.", filter_name.c_str()); + return false; + } + else{ + //ROS_INFO("config_filter_b : %f ,%f",(float)b_[0],(float)b_[1]); + ROS_INFO("Loaded IMU b"); + } + ///\todo check length + + // // Create the input and output buffers of the correct size. + temp_.resize(this->number_of_channels_); + + ROS_INFO("b_.size = %f",(float)b_.size()); + input_buffer_.reset(new filters::RealtimeCircularBuffer >(b_.size()-1, temp_)); + output_buffer_.reset(new filters::RealtimeCircularBuffer >(a_.size()-1, temp_)); + + // Prevent divide by zero while normalizing coeffs. + if ( a_[0] == 0) + { + ROS_ERROR("IMU a[0] can not equal 0."); + return false; + } else ROS_INFO( " IMU a_[0] = %f",(float)a_[0]); + + // Normalize the coeffs by a[0]. + if(a_[0] != 1) + { + ROS_INFO("Normalizing IMU coeff"); + for(uint32_t i = 0; i < b_.size(); i++) + { + b_[i] = (b_[i] / a_[0]); + } + for(uint32_t i = 1; i < a_.size(); i++) + { + a_[i] = (a_[i] / a_[0]); + } + a_[0] = (a_[0] / a_[0]); + } + + return true; +}; + + + void MultiChannelTransferFunctionFilter::IMU_Sensor_Filtering() + { + //ROS_INFO("force %f , %f , %f ",(float)data[0],(float)data[1],(float)data[2]); + std::vector data_in,data_out; + //data_in.resize(this->number_of_channels_); + data_out.resize(this->number_of_channels_); + + for (int i=0; iupdate(data_in,data_out); + //ROS_INFO("force %f,%f,%f ",(float)data_out[0],(float)data_out[1],(float)data_out[2]); + //ROS_INFO("torque %f,%f,%f ",(float)data_out[3],(float)data_out[4],(float)data_out[5]); + + imu_msg_out.header.stamp = ros::Time::now(); + imu_msg_out.header.frame_id=imu_sensor_frame_id; + //Guassian noise is applied to all measurements + imu_msg_out.orientation.x = data_out[0]; + imu_msg_out.orientation.y = data_out[1]; + imu_msg_out.orientation.z = data_out[2]; + imu_msg_out.orientation.w = data_out[3]; + + imu_msg_out.linear_acceleration.x = data_out[4]; + imu_msg_out.linear_acceleration.y = data_out[5]; + imu_msg_out.linear_acceleration.z = data_out[6]; + + imu_msg_out.angular_velocity.x = data_out[7]; + imu_msg_out.angular_velocity.y = data_out[8]; + imu_msg_out.angular_velocity.z = data_out[9]; + + } + + + + void MultiChannelTransferFunctionFilter::read(const sensor_msgs::Imu::ConstPtr &imu_msg) + { + imu_msg_in= *imu_msg; + this->data[0] = imu_msg->orientation.x; + this->data[1] = imu_msg->orientation.y; + this->data[2] = imu_msg->orientation.z; + this->data[3] = imu_msg->orientation.w; + + this->data[4] = imu_msg->linear_acceleration.x; + this->data[5] = imu_msg->linear_acceleration.y; + this->data[6] = imu_msg->linear_acceleration.z; + + this->data[7] = imu_msg->angular_velocity.x; + this->data[8] = imu_msg->angular_velocity.y; + this->data[9] = imu_msg->angular_velocity.z; + + } + + void MultiChannelTransferFunctionFilter::IMU_Sensor_Read(std::string filtering_imu_sensor_params_name) + { + ros::NodeHandle* rosnode = new ros::NodeHandle(); + + rosnode->setCallbackQueue(&subscriber_queue_); + + ros::Time last_ros_time_; + bool wait = true; + while (wait) + { + last_ros_time_ = ros::Time::now(); + if (last_ros_time_.toSec() > 0) + wait = false; + } + nh_.getParam(filtering_imu_sensor_params_name+"/imu_sensor_topic", imu_sensor_topic); + ROS_INFO_STREAM("filtering_imu_sensor_topic: "<< imu_sensor_topic); + // ros topic subscribtions + ros::SubscribeOptions Sub_IMU = + ros::SubscribeOptions::create( + imu_sensor_topic, 1,boost::bind(&MultiChannelTransferFunctionFilter::read, this, _1), + ros::VoidPtr(), rosnode->getCallbackQueue()); + //&Force_Torque_Sensor_Filtering::force_torque_sensor_State + + imu_data_subscriber = rosnode->subscribe(Sub_IMU); + + subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_)); + subscriber_spinner_->start(); + } + + + void MultiChannelTransferFunctionFilter::cleanup() + { + subscriber_spinner_->stop(); + } + + void MultiChannelTransferFunctionFilter::pub_IMU_filtered(ros::Publisher publisher) + { + imu_msg_out.orientation_covariance[0] = imu_msg_in.orientation_covariance[0]; + imu_msg_out.orientation_covariance[4] = imu_msg_in.orientation_covariance[4]; + imu_msg_out.orientation_covariance[8] = imu_msg_in.orientation_covariance[8]; + imu_msg_out.angular_velocity_covariance[0] = imu_msg_in.angular_velocity_covariance[0]; + imu_msg_out.angular_velocity_covariance[4] = imu_msg_in.angular_velocity_covariance[4]; + imu_msg_out.angular_velocity_covariance[8] = imu_msg_in.angular_velocity_covariance[8]; + imu_msg_out.linear_acceleration_covariance[0] = imu_msg_in.linear_acceleration_covariance[0]; + imu_msg_out.linear_acceleration_covariance[4] = imu_msg_in.linear_acceleration_covariance[4]; + imu_msg_out.linear_acceleration_covariance[8] = imu_msg_in.linear_acceleration_covariance[8]; + publisher.publish(imu_msg_out); + } + +} + + + + + int main(int argc, char** argv){ + + try{ + ROS_INFO("starting IMU filtering node"); + ros::init(argc, argv, "IMU_sensor_filtering"); + + ros::NodeHandle nh; + + std::string filtering_imu_sensor_params_name; + + nh.getParam("imu_sensor_params_name", filtering_imu_sensor_params_name); + ROS_INFO_STREAM("Filtering_imu_sensor_params_name: "<< filtering_imu_sensor_params_name); + + IMU_Sensor_Filtering::MultiChannelTransferFunctionFilter filter; + + filter.init(filtering_imu_sensor_params_name); + //filter.configure(); //filtering_ft_sensor_params_name+"/params/" + filter.config(); + + filter.IMU_Sensor_Read(filtering_imu_sensor_params_name); + //Force_Torque_Sensor_Filtering::MultiChannelTransferFunctionFilter::Force_Torque_Sensor_Read data_acq(filtering_ft_sensor_params_name); + //Force_Torque_Sensor_Filtering::MultiChannelTransferFunctionFilter::Force_Torque_Sensor_Read filter_acq(filtering_ft_sensor_params_name); + //filter_sub.init(filtering_ft_sensor_params_name); + //filters::SingleChannelTransferFunctionFilter filter; + + + + ros::NodeHandle filter_nh("IMU_filtering"); + + ros::Publisher Pub_IMU_ = nh.advertise(filter.imu_sensor_name+"/IMU_filtered",1,true); + + ros::AsyncSpinner spinner(2); + spinner.start(); + + ros::Rate loop_rate(1000); + + ros::Time last_time = ros::Time::now(); + + while (ros::ok()) + { + //ROS_INFO("force_torque_sensor_filtering is here"); + ros::Time current_time = ros::Time::now(); + ros::Duration elapsed_time = current_time - last_time; + last_time = current_time; + + //filter.update(data_in,data_out); + filter.IMU_Sensor_Filtering(); + filter.pub_IMU_filtered(Pub_IMU_); + loop_rate.sleep(); + } + + filter.cleanup(); + } + catch(...) + { + ROS_ERROR("Unhandled exception!"); + return -1; + } + + return 0; +} diff --git a/src/trans_function_filter.cpp b/src/trans_function_filter.cpp new file mode 100644 index 0000000..f8b2304 --- /dev/null +++ b/src/trans_function_filter.cpp @@ -0,0 +1,281 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Luca Gemma + * Team Pacman, + * Università di Pisa, Centro Piaggio + * All rights reserved. + * + * 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 Technische Universität Darmstadt nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 THE + * COPYRIGHT OWNER 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. + * + *********************************************************************/ +#include +#include +#include + + +namespace Force_Torque_Sensor_Filtering +{ + + void MultiChannelTransferFunctionFilter::init(std::string filtering_ft_sensor_params_name) + { + nh_.getParam(filtering_ft_sensor_params_name+"/ft_sensor_name", ft_sensor_name); + nh_.getParam(filtering_ft_sensor_params_name+"/ft_sensor_frame_id",ft_sensor_frame_id); + //ROS_INFO_STREAM("filtering_name: "<< ft_sensor_name); + //ROS_INFO_STREAM("filtering_ft_sensor_frame_id: "<< ft_sensor_frame_id); + + nh_.getParam("LowPass/name", filter_name); + ROS_INFO_STREAM("filter_name: "<< filter_name); + nh_.getParam("LowPass/type", filter_type); + ROS_INFO_STREAM("filter_type: "<< filter_type); + + //nh_.getParam("LowPass/params/a", a_); + //ROS_INFO_STREAM("filter_a: "<< a_); + + //nh_.getParam("LowPass/params/b", b_); + //ROS_INFO_STREAM("filter_b: "<< b_); + + //number_of_channels_ = 6; + //this->number_of_channels_=6; + this->filter_type_=filter_name; + this->filter_type_=filter_type; + this->number_of_channels_=6; + + } + + bool MultiChannelTransferFunctionFilter::config() +{ + // Parse a and b into a std::vector. + + if (!nh_.getParam("LowPass/params/a", a_)) + { + ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", filter_name.c_str()); + return false; + } + else{ + //ROS_INFO("config_filter_a : %f ,%f",(float)a_[0],(float)a_[1]); + ROS_INFO("Loaded a"); + } + + if (!nh_.getParam("LowPass/params/b", b_)) + { + ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", filter_name.c_str()); + return false; + } + else{ + //ROS_INFO("config_filter_b : %f ,%f",(float)b_[0],(float)b_[1]); + ROS_INFO("Loaded b"); + } + ///\todo check length + + // // Create the input and output buffers of the correct size. + temp_.resize(this->number_of_channels_); + + ROS_INFO("b_.size = %f",(float)b_.size()); + input_buffer_.reset(new filters::RealtimeCircularBuffer >(b_.size()-1, temp_)); + output_buffer_.reset(new filters::RealtimeCircularBuffer >(a_.size()-1, temp_)); + + // Prevent divide by zero while normalizing coeffs. + if ( a_[0] == 0) + { + ROS_ERROR("a[0] can not equal 0."); + return false; + } else ROS_INFO("a_[0] = %f",(float)a_[0]); + + // Normalize the coeffs by a[0]. + if(a_[0] != 1) + { + ROS_INFO("Normalizing coeff"); + for(uint32_t i = 0; i < b_.size(); i++) + { + b_[i] = (b_[i] / a_[0]); + } + for(uint32_t i = 1; i < a_.size(); i++) + { + a_[i] = (a_[i] / a_[0]); + } + a_[0] = (a_[0] / a_[0]); + } + + return true; +}; + + + void MultiChannelTransferFunctionFilter::Force_Torque_Sensor_Filtering() + { + //ROS_INFO("force %f , %f , %f ",(float)data[0],(float)data[1],(float)data[2]); + std::vector data_in,data_out; + //data_in.resize(this->number_of_channels_); + data_out.resize(this->number_of_channels_); + + for (int i=0; i<6; ++i) { + data_in.push_back(data[i]); + //ROS_INFO("force %f ",(float)data[i]); + } + + //ROS_INFO("force %f,%f,%f ",(float)data_in[0],(float)data_in[1],(float)data_in[2]); + //ROS_INFO("torque %f,%f,%f ",(float)data_in[3],(float)data_in[4],(float)data_in[5]); + + this->update(data_in,data_out); + //ROS_INFO("force %f,%f,%f ",(float)data_out[0],(float)data_out[1],(float)data_out[2]); + //ROS_INFO("torque %f,%f,%f ",(float)data_out[3],(float)data_out[4],(float)data_out[5]); + + WrenchStamped_out.wrench.force.x = data_out[0]; + WrenchStamped_out.wrench.force.y = data_out[1]; + WrenchStamped_out.wrench.force.z = data_out[2]; + WrenchStamped_out.wrench.torque.x = data_out[3]; + WrenchStamped_out.wrench.torque.y = data_out[4]; + WrenchStamped_out.wrench.torque.z = data_out[5]; + + WrenchStamped_out.header.stamp = ros::Time::now(); + WrenchStamped_out.header.frame_id = ft_sensor_frame_id; + + //Pub_Wrench_.publish(WrenchStamped_out); + + } + + + + void MultiChannelTransferFunctionFilter::read(const geometry_msgs::WrenchStamped::ConstPtr &_ws) + { + WrenchStamped = *_ws; + data[0] = WrenchStamped.wrench.force.x; + data[1] = WrenchStamped.wrench.force.y; + data[2] = WrenchStamped.wrench.force.z; + data[3] = WrenchStamped.wrench.torque.x; + data[4] = WrenchStamped.wrench.torque.y; + data[5] = WrenchStamped.wrench.torque.z; + } + + void MultiChannelTransferFunctionFilter::Force_Torque_Sensor_Read(std::string filtering_ft_sensor_params_name) + { + ros::NodeHandle* rosnode = new ros::NodeHandle(); + + rosnode->setCallbackQueue(&subscriber_queue_); + + ros::Time last_ros_time_; + bool wait = true; + while (wait) + { + last_ros_time_ = ros::Time::now(); + if (last_ros_time_.toSec() > 0) + wait = false; + } + nh_.getParam(filtering_ft_sensor_params_name+"/ft_sensor_topic", ft_sensor_topic); + ROS_INFO_STREAM("filtering_ft_sensor_topic: "<< ft_sensor_topic); + // ros topic subscribtions + ros::SubscribeOptions Sub_Wrench = + ros::SubscribeOptions::create( + ft_sensor_topic, 1,boost::bind(&MultiChannelTransferFunctionFilter::read, this, _1), + ros::VoidPtr(), rosnode->getCallbackQueue()); + //&Force_Torque_Sensor_Filtering::force_torque_sensor_State + + Sub_Wrench_ = rosnode->subscribe(Sub_Wrench); + + subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_)); + subscriber_spinner_->start(); + } + + + void MultiChannelTransferFunctionFilter::cleanup() + { + subscriber_spinner_->stop(); + } + + void MultiChannelTransferFunctionFilter::pub_Wrench_filtered(ros::Publisher publisher) + { + publisher.publish(WrenchStamped_out); + } + +} + + + + + int main(int argc, char** argv){ + + try{ + ROS_INFO("starting force torque sensor filtering"); + ros::init(argc, argv, "force_torque_sensor_filtering"); + + ros::NodeHandle nh; + + std::string filtering_ft_sensor_params_name; + + nh.getParam("ft_sensor_params_name", filtering_ft_sensor_params_name); + ROS_INFO_STREAM("Filtering_ft_sensor_params_name: "<< filtering_ft_sensor_params_name); + + Force_Torque_Sensor_Filtering::MultiChannelTransferFunctionFilter filter; + + filter.init(filtering_ft_sensor_params_name); + //filter.configure(); //filtering_ft_sensor_params_name+"/params/" + filter.config(); + + filter.Force_Torque_Sensor_Read(filtering_ft_sensor_params_name); + //Force_Torque_Sensor_Filtering::MultiChannelTransferFunctionFilter::Force_Torque_Sensor_Read data_acq(filtering_ft_sensor_params_name); + //Force_Torque_Sensor_Filtering::MultiChannelTransferFunctionFilter::Force_Torque_Sensor_Read filter_acq(filtering_ft_sensor_params_name); + //filter_sub.init(filtering_ft_sensor_params_name); + //filters::SingleChannelTransferFunctionFilter filter; + + + + ros::NodeHandle filter_nh("force_torque_sensor_filtering"); + + ros::Publisher Pub_Wrench_ = nh.advertise(filter.ft_sensor_name+"/force_torque_sensor_filtered",1,true); + + ros::AsyncSpinner spinner(2); + spinner.start(); + + ros::Rate loop_rate(1000); + + ros::Time last_time = ros::Time::now(); + + while (ros::ok()) + { + //ROS_INFO("force_torque_sensor_filtering is here"); + ros::Time current_time = ros::Time::now(); + ros::Duration elapsed_time = current_time - last_time; + last_time = current_time; + + //filter.update(data_in,data_out); + filter.Force_Torque_Sensor_Filtering(); + filter.pub_Wrench_filtered(Pub_Wrench_); + loop_rate.sleep(); + } + + filter.cleanup(); + } + catch(...) + { + ROS_ERROR("Unhandled exception!"); + return -1; + } + + return 0; +}