gazebo_ros_force.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 /*
18  * Desc: A dynamic controller plugin that performs generic force interface.
19  * Author: John Hsu
20  * Date: 24 Sept 2008
21  */
22 
23 #ifndef GAZEBO_ROS_FORCE_HH
24 #define GAZEBO_ROS_FORCE_HH
25 
26 #include <string>
27 
28 // Custom Callback Queue
29 #include <ros/callback_queue.h>
30 #include <ros/subscribe_options.h>
31 #include <geometry_msgs/Wrench.h>
32 
33 #include <ros/ros.h>
34 #include <boost/thread.hpp>
35 #include <boost/thread/mutex.hpp>
36 
37 #include <gazebo/physics/physics.hh>
38 #include <gazebo/transport/TransportTypes.hh>
39 #include <gazebo/common/Plugin.hh>
40 #include <gazebo/common/Events.hh>
41 
42 
43 namespace gazebo
44 {
47 
71 class GazeboRosForce : public ModelPlugin
72 {
74  public: GazeboRosForce();
75 
77  public: virtual ~GazeboRosForce();
78 
79  // Documentation inherited
80  protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
81 
82  // Documentation inherited
83  protected: virtual void UpdateChild();
84 
87  private: void UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg);
88 
90  private: void QueueThread();
91 
93  private: physics::WorldPtr world_;
94 
96  private: physics::LinkPtr link_;
97 
101 
103  private: boost::mutex lock_;
104 
106  private: std::string topic_name_;
108  private: std::string link_name_;
109 
111  private: std::string robot_namespace_;
112 
113  // Custom Callback Queue
116  private: boost::thread callback_queue_thread_;
118  private: geometry_msgs::Wrench wrench_msg_;
119 
120  // Pointer to the update event connection
121  private: event::ConnectionPtr update_connection_;
122 };
124 }
126 #endif
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
geometry_msgs::Wrench wrench_msg_
Container for the wrench force that this plugin exerts on the body.
void QueueThread()
The custom callback queue thread function.
ros::CallbackQueue queue_
GazeboRosForce()
Constructor.
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
std::string link_name_
The Link this plugin is attached to, and will exert forces on.
physics::WorldPtr world_
A pointer to the gazebo world.
virtual ~GazeboRosForce()
Destructor.
std::string robot_namespace_
for setting ROS name space
physics::LinkPtr link_
A pointer to the Link, where force is applied.
event::ConnectionPtr update_connection_
void UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr &_msg)
call back when a Wrench message is published
std::string topic_name_
ROS Wrench topic name inputs.
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
boost::thread callback_queue_thread_
Thead object for the running callback Thread.


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52