Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

add force sensor plugin #675

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add force sensor plugin #675

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 24 additions & 0 deletions rotors_gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
# BUILD_MAVLINK_INTERFACE_PLUGIN bool Build mavlink_interface_plugin if mavlink found (requires mav dependency).
# BUILD_OCTOMAP_PLUGIN bool Build the optical map plugin (requires Octomap).
# BUILD_OPTICAL_FLOW_PLUGIN bool Build the optical flow plugin (requires OpenCV).
# BUILD_AERIAL_MANIPULATION_PLUGINS bool Build plugins related to aerial manipulation
# MAVLINK_HEADER_DIR string Location of MAVLink header files. If not provided, this CMakeLists.txt file will
# search the default locations (e.g. ROS) for them. This variable is only required
# if BUILD_MAVLINK_INTERFACE_PLUGIN=TRUE.
Expand All @@ -28,6 +29,11 @@ if(NOT DEFINED BUILD_OCTOMAP_PLUGIN)
set(BUILD_OCTOMAP_PLUGIN FALSE)
endif()

if(NOT DEFINED BUILD_AERIAL_MANIPULATION_PLUGINS)
message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS variable not provided, setting to TRUE.")
set(BUILD_AERIAL_MANIPULATION_PLUGINS TRUE)
endif()

if(NOT DEFINED BUILD_OPTICAL_FLOW_PLUGIN)
message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN variable not provided, setting to FALSE.")
set(BUILD_OPTICAL_FLOW_PLUGIN FALSE)
Expand Down Expand Up @@ -84,6 +90,12 @@ else ()
message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN = FALSE, NOT building gazebo_optical_flow_plugin.")
endif ()

if(BUILD_AERIAL_MANIPULATION_PLUGINS)
message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS = TRUE, building aerial manipulation plugins.")
else ()
message(STATUS "BUILD_AERIAL_MANIPULATION_PLUGINS = FALSE, NOT building aerial manipulation plugins.")
endif ()

if(NO_ROS)
message(STATUS "NO_ROS = TRUE, building rotors_gazebo_plugins WITHOUT any ROS dependancies.")
else()
Expand Down Expand Up @@ -456,6 +468,18 @@ if(BUILD_OCTOMAP_PLUGIN)
endif()
list(APPEND targets_to_install rotors_gazebo_octomap_plugin)
endif()
#====================================== Aerial Manipulation PLUGINs ==========================================//

if(BUILD_AERIAL_MANIPULATION_PLUGINS)

add_library(rotors_gazebo_force_sensor_plugin SHARED src/gazebo_force_sensor_plugin.cpp)
target_link_libraries(rotors_gazebo_force_sensor_plugin ${target_linking_LIBRARIES})
if (NOT NO_ROS)
add_dependencies(rotors_gazebo_force_sensor_plugin ${catkin_EXPORTED_TARGETS})
endif()
list(APPEND targets_to_install rotors_gazebo_force_sensor_plugin)

endif()

#======================================= ODOMETRY PLUGIN ========================================//
if (NOT NO_ROS)
Expand Down
220 changes: 220 additions & 0 deletions rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_force_sensor_plugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,220 @@
/*
* Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
* Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
* Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
* Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
* Copyright 2015 Simone Comari, ASL, ETH Zurich, Switzerland
*
* 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 ROTORS_GAZEBO_PLUGINS_FORCE_SENSOR_PLUGIN_H
#define ROTORS_GAZEBO_PLUGINS_FORCE_SENSOR_PLUGIN_H

#include <cmath>
#include <deque>
#include <random>
#include <stdio.h>

#include <rotors_gazebo_plugins/common.h>

#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Wrench.h>
#include <geometry_msgs/WrenchStamped.h>

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <tf/transform_broadcaster.h>

#include <boost/thread.hpp>


namespace gazebo {
// Default values
//static const std::string kDefaultNamespace = "";
static const std::string kDefaultPublishFrameId = "world";
static const std::string kDefaultParentFrameId = "world";
static const std::string kDefaultReferenceFrameId = "world";
static const std::string kDefaultLinkName = "f2g_sensor_link";
static const std::string kDefaultForceSensorPubTopic = "force_sensor";
static const std::string kDefaultForceSensorTruthPubTopic = "force_sensor_truth";
static const std::string kDefaultWrenchVectorPubTopic = "wrench_vector";

static constexpr int kDefaultMeasurementDelay = 0;
static constexpr int kDefaultMeasurementDivisor = 1;
static constexpr int kDefaultGazeboSequence = 0;
static constexpr int kDefaultWrenchSequence = 0;
static constexpr double kDefaultUnknownDelay = 0.0;
static constexpr double kDefaultCutOffFrequency = 10.0;
static constexpr bool kDefaultLinforceMeasEnabled = true;
static constexpr bool kDefaultTorqueMeasEnabled = true;
static constexpr bool kDefaultDispWrenchVector = true;


class FirstOrderFilterJointWrench {
public:
FirstOrderFilterJointWrench(double timeConstantUp, double timeConstantDown, physics::JointWrench initialState):
previousState_(initialState) {
// Initialize filters, one for each wrench element.
force_x_filter_.reset(new FirstOrderFilter<double>(timeConstantUp, timeConstantDown, initialState.body1Force[0]));
force_y_filter_.reset(new FirstOrderFilter<double>(timeConstantUp, timeConstantDown, initialState.body1Force[1]));
force_z_filter_.reset(new FirstOrderFilter<double>(timeConstantUp, timeConstantDown, initialState.body1Force[2]));
torque_x_filter_.reset(new FirstOrderFilter<double>(timeConstantUp, timeConstantDown, initialState.body1Torque[0]));
torque_y_filter_.reset(new FirstOrderFilter<double>(timeConstantUp, timeConstantDown, initialState.body1Torque[1]));
torque_z_filter_.reset(new FirstOrderFilter<double>(timeConstantUp, timeConstantDown, initialState.body1Torque[2]));
}

physics::JointWrench updateFilter(physics::JointWrench inputState, double samplingTime) {
/*
This method will apply a first order filter on each wrench element of the inputState.
*/
physics::JointWrench outputState;

outputState.body1Force[0] = force_x_filter_->updateFilter(inputState.body1Force[0],samplingTime);
outputState.body1Force[1] = force_y_filter_->updateFilter(inputState.body1Force[1],samplingTime);
outputState.body1Force[2] = force_z_filter_->updateFilter(inputState.body1Force[2],samplingTime);
outputState.body1Torque[0] = torque_x_filter_->updateFilter(inputState.body1Torque[0],samplingTime);
outputState.body1Torque[1] = torque_y_filter_->updateFilter(inputState.body1Torque[1],samplingTime);
outputState.body1Torque[2] = torque_z_filter_->updateFilter(inputState.body1Torque[2],samplingTime);

previousState_ = outputState;
return outputState;
}

~FirstOrderFilterJointWrench() {}

protected:
physics::JointWrench previousState_;
std::unique_ptr<FirstOrderFilter<double>> force_x_filter_;
std::unique_ptr<FirstOrderFilter<double>> force_y_filter_;
std::unique_ptr<FirstOrderFilter<double>> force_z_filter_;
std::unique_ptr<FirstOrderFilter<double>> torque_x_filter_;
std::unique_ptr<FirstOrderFilter<double>> torque_y_filter_;
std::unique_ptr<FirstOrderFilter<double>> torque_z_filter_;
};


class GazeboForceSensorPlugin : public ModelPlugin {

public:
typedef std::normal_distribution<> NormalDistribution;
typedef std::uniform_real_distribution<> UniformDistribution;
typedef std::deque<std::pair<int, geometry_msgs::WrenchStamped> > WrenchQueue;

GazeboForceSensorPlugin()
: ModelPlugin(),
random_generator_(random_device_()),
publish_frame_id_(kDefaultPublishFrameId),
parent_frame_id_(kDefaultParentFrameId),
reference_frame_id_(kDefaultReferenceFrameId),
link_name_(kDefaultLinkName),
force_sensor_pub_topic_(kDefaultForceSensorPubTopic),
force_sensor_truth_pub_topic_(kDefaultForceSensorTruthPubTopic),
wrench_vector_pub_topic_(kDefaultWrenchVectorPubTopic),
measurement_delay_(kDefaultMeasurementDelay),
measurement_divisor_(kDefaultMeasurementDivisor),
unknown_delay_(kDefaultUnknownDelay),
cutoff_frequency_(kDefaultCutOffFrequency),
lin_force_meas_enabled_(kDefaultLinforceMeasEnabled),
torque_meas_enabled_(kDefaultTorqueMeasEnabled),
disp_wrench_vector_(kDefaultDispWrenchVector),
gazebo_sequence_(kDefaultGazeboSequence),
wrench_sequence_(kDefaultWrenchSequence),
prev_sim_time_(0.0),
sampling_time_(0.001),
node_handle_(NULL) {}

~GazeboForceSensorPlugin();

void InitializeParams();
void Publish();


protected:
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
void OnUpdate(const common::UpdateInfo& /*_info*/);
void PrintJointWrench(physics::JointWrench jw_);


private:
WrenchQueue wrench_queue_;

std::string namespace_;
std::string force_sensor_pub_topic_;
std::string force_sensor_truth_pub_topic_;
std::string wrench_vector_pub_topic_;
std::string publish_frame_id_;
std::string parent_frame_id_;
std::string reference_frame_id_;
std::string link_name_;

NormalDistribution linear_force_n_[3];
NormalDistribution torque_n_[3];
UniformDistribution linear_force_u_[3];
UniformDistribution torque_u_[3];

geometry_msgs::WrenchStamped wrench_msg_;

int measurement_delay_;
int measurement_divisor_;
int gazebo_sequence_;
int wrench_sequence_;
double prev_sim_time_;
double sampling_time_;
double unknown_delay_;
double cutoff_frequency_;
bool lin_force_meas_enabled_;
bool torque_meas_enabled_;
bool disp_wrench_vector_;

std::random_device random_device_;
std::mt19937 random_generator_;

ros::NodeHandle* node_handle_;
ros::Publisher lin_force_sensor_pub_;
ros::Publisher lin_force_sensor_truth_pub_;
ros::Publisher ang_force_sensor_pub_;
ros::Publisher ang_force_sensor_truth_pub_;
ros::Publisher wrench_vector_pub_;

tf::Transform tf_;
tf::TransformBroadcaster transform_broadcaster_;

physics::WorldPtr world_;
physics::ModelPtr model_;
physics::LinkPtr link_;
physics::LinkPtr parent_link_;
physics::LinkPtr reference_link_;
physics::JointPtr joint_;
physics::JointWrench joint_wrench_;

std::unique_ptr<FirstOrderFilterJointWrench> sensor_data_filter_;

/// \brief Pointer to the update event connection.
event::ConnectionPtr updateConnection_;

boost::thread callback_queue_thread_;

void QueueThread();
};

}

#endif /* ROTORS_GAZEBO_PLUGINS_FORCE_SENSOR_PLUGIN_H */
Loading