61 if (_sdf->HasElement(
"robotNamespace"))
62 this->
robot_namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>() +
"/";
64 if (!_sdf->HasElement(
"jointName"))
66 ROS_FATAL_NAMED(
"ft_sensor",
"ft_sensor plugin missing <jointName>, cannot proceed");
70 this->
joint_name_ = _sdf->GetElement(
"jointName")->Get<std::string>();
85 if (!_sdf->HasElement(
"topicName"))
87 ROS_FATAL_NAMED(
"ft_sensor",
"ft_sensor plugin missing <topicName>, cannot proceed");
91 this->
topic_name_ = _sdf->GetElement(
"topicName")->Get<std::string>();
93 if (!_sdf->HasElement(
"gaussianNoise"))
95 ROS_INFO_NAMED(
"ft_sensor",
"imu plugin missing <gaussianNoise>, defaults to 0.0");
101 if (!_sdf->HasElement(
"updateRate"))
103 ROS_DEBUG_NAMED(
"ft_sensor",
"ft_sensor plugin missing <updateRate>, defaults to 0.0" 104 " (as fast as possible)");
108 this->
update_rate_ = _sdf->GetElement(
"updateRate")->Get<
double>();
113 ROS_FATAL_STREAM_NAMED(
"ft_sensor",
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 114 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
160 #if GAZEBO_MAJOR_VERSION >= 8 161 common::Time cur_time = this->
world_->SimTime();
163 common::Time cur_time = this->
world_->GetSimTime();
174 physics::JointWrench wrench;
175 ignition::math::Vector3d torque;
176 ignition::math::Vector3d force;
183 wrench = this->
joint_->GetForceTorque(0);
184 #if GAZEBO_MAJOR_VERSION >= 8 185 force = wrench.body2Force;
186 torque = wrench.body2Torque;
188 force = wrench.body2Force.Ign();
189 torque = wrench.body2Torque.Ign();
196 #if GAZEBO_MAJOR_VERSION >= 8 212 this->
lock_.unlock();
226 double U =
static_cast<double>(rand_r(&this->
seed)) /
227 static_cast<double>(RAND_MAX);
230 double V =
static_cast<double>(rand_r(&this->
seed)) /
231 static_cast<double>(RAND_MAX);
233 double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V);
247 static const double timeout = 0.01;
virtual void UpdateChild()
Update the controller.
#define ROS_INFO_NAMED(name,...)
std::string topic_name_
ROS WrenchStamped topic name.
virtual ~GazeboRosFT()
Destructor.
int ft_connect_count_
: keep track of number of connections
void publish(const boost::shared_ptr< M > &message) const
std::string robot_namespace_
for setting ROS name space
physics::JointPtr joint_
A pointer to the Gazebo joint.
geometry_msgs::WrenchStamped wrench_msg_
ROS WrenchStamped message.
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
ROSCPP_DECL bool isInitialized()
ros::CallbackQueue queue_
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
double GaussianKernel(double mu, double sigma)
Gaussian noise generator.
boost::mutex lock_
A mutex to lock access to fields that are used in message callbacks.
boost::thread callback_queue_thread_
std::string resolve(const std::string &prefix, const std::string &frame_name)
double gaussian_noise_
Gaussian noise.
#define ROS_DEBUG_NAMED(name,...)
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
physics::WorldPtr world_
A pointer to the Gazebo world.
#define ROS_FATAL_STREAM_NAMED(name, args)
event::ConnectionPtr update_connection_
physics::ModelPtr model_
A pointer to the Gazebo model.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
physics::LinkPtr parent_link_
A pointer to the Gazebo parent link.
bool getParam(const std::string &key, std::string &s) const
#define ROS_FATAL_NAMED(name,...)
physics::LinkPtr child_link_
A pointer to the Gazebo child link.
common::Time last_time_
save last_time
GazeboRosFT()
Constructor.
std::string joint_name_
store bodyname
std::string frame_name_
ROS frame transform name to use in the image message header. FIXME: extract link name directly...
boost::shared_ptr< void > VoidPtr