Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "rotors_gazebo_plugins/gazebo_multirotor_base_plugin.h"
00024
00025
00026 #include <ctime>
00027
00028 #include "ConnectGazeboToRosTopic.pb.h"
00029
00030 namespace gazebo {
00031
00032 GazeboMultirotorBasePlugin::~GazeboMultirotorBasePlugin() {
00033
00034 }
00035
00036 void GazeboMultirotorBasePlugin::Load(physics::ModelPtr _model,
00037 sdf::ElementPtr _sdf) {
00038 if (kPrintOnPluginLoad) {
00039 gzdbg << __FUNCTION__ << "() called." << std::endl;
00040 }
00041
00042 model_ = _model;
00043 world_ = model_->GetWorld();
00044 namespace_.clear();
00045
00046 getSdfParam<std::string>(_sdf, "robotNamespace", namespace_, namespace_,
00047 true);
00048 getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_, true);
00049 getSdfParam<std::string>(_sdf, "motorPubTopic", actuators_pub_topic_,
00050 actuators_pub_topic_);
00051 getSdfParam<double>(_sdf, "rotorVelocitySlowdownSim",
00052 rotor_velocity_slowdown_sim_,
00053 rotor_velocity_slowdown_sim_);
00054
00055 node_handle_ = gazebo::transport::NodePtr(new transport::Node());
00056
00057
00058 node_handle_->Init();
00059
00060 frame_id_ = link_name_;
00061
00062 link_ = model_->GetLink(link_name_);
00063 if (link_ == NULL)
00064 gzthrow("[gazebo_multirotor_base_plugin] Couldn't find specified link \""
00065 << link_name_ << "\".");
00066
00067
00068
00069 update_connection_ = event::Events::ConnectWorldUpdateBegin(
00070 boost::bind(&GazeboMultirotorBasePlugin::OnUpdate, this, _1));
00071
00072 child_links_ = link_->GetChildJointsLinks();
00073 for (unsigned int i = 0; i < child_links_.size(); i++) {
00074 std::string link_name = child_links_[i]->GetScopedName();
00075
00076
00077 int pos = link_name.find("rotor_");
00078 if (pos != link_name.npos) {
00079 std::string motor_number_str = link_name.substr(pos + 6);
00080 unsigned int motor_number = std::stoi(motor_number_str);
00081 std::string joint_name = child_links_[i]->GetName() + "_joint";
00082 physics::JointPtr joint = this->model_->GetJoint(joint_name);
00083 motor_joints_.insert(MotorNumberToJointPair(motor_number, joint));
00084 }
00085 }
00086 }
00087
00088
00089 void GazeboMultirotorBasePlugin::OnUpdate(const common::UpdateInfo& _info) {
00090 if (kPrintOnUpdates) {
00091 gzdbg << __FUNCTION__ << "() called." << std::endl;
00092 }
00093
00094 if (!pubs_and_subs_created_) {
00095 CreatePubsAndSubs();
00096 pubs_and_subs_created_ = true;
00097 }
00098
00099
00100 common::Time now = world_->SimTime();
00101
00102 actuators_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
00103 actuators_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
00104 actuators_msg_.mutable_header()->set_frame_id(frame_id_);
00105
00106 joint_state_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
00107 joint_state_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
00108 joint_state_msg_.mutable_header()->set_frame_id(frame_id_);
00109
00110 actuators_msg_.clear_angular_velocities();
00111
00112 joint_state_msg_.clear_name();
00113 joint_state_msg_.clear_position();
00114
00115 MotorNumberToJointMap::iterator m;
00116 for (m = motor_joints_.begin(); m != motor_joints_.end(); ++m) {
00117 double motor_rot_vel =
00118 m->second->GetVelocity(0) * rotor_velocity_slowdown_sim_;
00119
00120 actuators_msg_.add_angular_velocities(motor_rot_vel);
00121
00122 joint_state_msg_.add_name(m->second->GetName());
00123 joint_state_msg_.add_position(m->second->Position(0));
00124 }
00125
00126 joint_state_pub_->Publish(joint_state_msg_);
00127 motor_pub_->Publish(actuators_msg_);
00128 }
00129
00130 void GazeboMultirotorBasePlugin::CreatePubsAndSubs() {
00131
00132 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
00133 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00134 "~/" + kConnectGazeboToRosSubtopic, 1);
00135
00136 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
00137
00138
00139
00140
00141 motor_pub_ = node_handle_->Advertise<gz_sensor_msgs::Actuators>(
00142 "~/" + namespace_ + "/" + actuators_pub_topic_, 10);
00143
00144
00145 connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + model_->GetName() +
00146 "/" + actuators_pub_topic_);
00147 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00148 actuators_pub_topic_);
00149 connect_gazebo_to_ros_topic_msg.set_msgtype(
00150 gz_std_msgs::ConnectGazeboToRosTopic::ACTUATORS);
00151 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00152 true);
00153
00154
00155
00156
00157 joint_state_pub_ = node_handle_->Advertise<gz_sensor_msgs::JointState>(
00158 "~/" + namespace_ + "/" + joint_state_pub_topic_, 1);
00159
00160
00161 connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00162 joint_state_pub_topic_);
00163 connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00164 joint_state_pub_topic_);
00165 connect_gazebo_to_ros_topic_msg.set_msgtype(
00166 gz_std_msgs::ConnectGazeboToRosTopic::JOINT_STATE);
00167 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00168 true);
00169 }
00170
00171 GZ_REGISTER_MODEL_PLUGIN(GazeboMultirotorBasePlugin);
00172
00173 }
rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43