gazebo_multirotor_base_plugin.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
3  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
4  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
5  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
6  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
7  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
8  *
9  * Licensed under the Apache License, Version 2.0 (the "License");
10  * you may not use this file except in compliance with the License.
11  * You may obtain a copy of the License at
12  *
13  * http://www.apache.org/licenses/LICENSE-2.0
14 
15  * Unless required by applicable law or agreed to in writing, software
16  * distributed under the License is distributed on an "AS IS" BASIS,
17  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
18  * See the License for the specific language governing permissions and
19  * limitations under the License.
20  */
21 
22 
23 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_MULTIROTOR_BASE_PLUGIN_H
24 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_MULTIROTOR_BASE_PLUGIN_H
25 
26 #include <string>
27 
28 #include <gazebo/common/common.hh>
29 #include <gazebo/common/Plugin.hh>
30 #include <gazebo/gazebo.hh>
31 #include <gazebo/physics/physics.hh>
32 
33 #include <mav_msgs/default_topics.h> // This comes from the mav_comm repo
34 
35 #include "Actuators.pb.h"
36 #include "JointState.pb.h"
37 
39 
40 namespace gazebo {
41 
42 // Default values
43 static const std::string kDefaultLinkName = "base_link";
44 static const std::string kDefaultFrameId = "base_link";
45 static const std::string kDefaultJointStatePubTopic = "joint_states";
46 
48 class GazeboMultirotorBasePlugin : public ModelPlugin {
49  typedef std::map<const unsigned int, const physics::JointPtr> MotorNumberToJointMap;
50  typedef std::pair<const unsigned int, const physics::JointPtr> MotorNumberToJointPair;
51  public:
53  : ModelPlugin(),
55  joint_state_pub_topic_(kDefaultJointStatePubTopic),
56  actuators_pub_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT),
57  link_name_(kDefaultLinkName),
58  frame_id_(kDefaultFrameId),
60  node_handle_(NULL),
61  pubs_and_subs_created_(false) {}
62 
64 
65  protected:
66 
70  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
71 
74  void OnUpdate(const common::UpdateInfo& /*_info*/);
75 
76  private:
77 
81 
86  void CreatePubsAndSubs();
87 
89  event::ConnectionPtr update_connection_;
90 
91  physics::WorldPtr world_;
92  physics::ModelPtr model_;
93  physics::LinkPtr link_;
94 
95  physics::Link_V child_links_;
96 
97  MotorNumberToJointMap motor_joints_;
98 
99  std::string namespace_;
101  std::string actuators_pub_topic_;
102  std::string link_name_;
103  std::string frame_id_;
105 
106  gazebo::transport::PublisherPtr motor_pub_;
107 
109  gz_sensor_msgs::Actuators actuators_msg_;
110 
111  gazebo::transport::PublisherPtr joint_state_pub_;
112 
114  gz_sensor_msgs::JointState joint_state_msg_;
115 
116  gazebo::transport::NodePtr node_handle_;
117 };
118 
119 } // namespace gazebo
120 
121 #endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_MULTIROTOR_BASE_PLUGIN_H
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Load the plugin.
static const std::string kDefaultFrameId
static constexpr double kDefaultRotorVelocitySlowdownSim
Definition: common.h:49
static const std::string kDefaultLinkName
This plugin publishes the motor speeds of your multirotor model.
static const std::string kDefaultJointStatePubTopic
event::ConnectionPtr update_connection_
Pointer to the update event connection.
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
gazebo::transport::PublisherPtr joint_state_pub_
static const std::string kDefaultNamespace
Definition: common.h:48
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
std::pair< const unsigned int, const physics::JointPtr > MotorNumberToJointPair
std::map< const unsigned int, const physics::JointPtr > MotorNumberToJointMap
gazebo::transport::PublisherPtr motor_pub_
void OnUpdate(const common::UpdateInfo &)
Called when the world is updated.


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Mon Feb 28 2022 23:39:03