gazebo_controller_interface.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 #include "rotors_gazebo_plugins/gazebo_controller_interface.h"
00023 
00024 #include "ConnectGazeboToRosTopic.pb.h"
00025 #include "ConnectRosToGazeboTopic.pb.h"
00026 
00027 namespace gazebo {
00028 
00029 GazeboControllerInterface::~GazeboControllerInterface() {
00030 }
00031 
00032 void GazeboControllerInterface::Load(physics::ModelPtr _model,
00033                                      sdf::ElementPtr _sdf) {
00034   if (kPrintOnPluginLoad) {
00035     gzdbg << __FUNCTION__ << "() called." << std::endl;
00036   }
00037 
00038   // Store the pointer to the model.
00039   model_ = _model;
00040 
00041   world_ = model_->GetWorld();
00042 
00043   namespace_.clear();
00044 
00045   //==============================================//
00046   //========== READ IN PARAMS FROM SDF ===========//
00047   //==============================================//
00048 
00049   if (_sdf->HasElement("robotNamespace")) {
00050     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00051   } else {
00052     gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n";
00053   }
00054 
00055   node_handle_ = gazebo::transport::NodePtr(new transport::Node());
00056 
00057   // Initialise with default namespace (typically /gazebo/default/)
00058   node_handle_->Init();
00059 
00060   getSdfParam<std::string>(_sdf, "commandMotorSpeedSubTopic",
00061                            command_motor_speed_sub_topic_,
00062                            command_motor_speed_sub_topic_);
00063   getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic",
00064                            motor_velocity_reference_pub_topic_,
00065                            motor_velocity_reference_pub_topic_);
00066 
00067   // Listen to the update event. This event is broadcast every
00068   // simulation iteration.
00069   updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00070       boost::bind(&GazeboControllerInterface::OnUpdate, this, _1));
00071 }
00072 
00073 void GazeboControllerInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
00074   if (kPrintOnUpdates) {
00075     gzdbg << __FUNCTION__ << "() called." << std::endl;
00076   }
00077 
00078   if (!pubs_and_subs_created_) {
00079     CreatePubsAndSubs();
00080     pubs_and_subs_created_ = true;
00081   }
00082 
00083   if (!received_first_reference_) {
00084     return;
00085   }
00086 
00087   common::Time now = world_->SimTime();
00088 
00089   gz_sensor_msgs::Actuators turning_velocities_msg;
00090 
00091   for (int i = 0; i < input_reference_.size(); i++) {
00092     turning_velocities_msg.add_angular_velocities((double)input_reference_[i]);
00093   }
00094 
00095   turning_velocities_msg.mutable_header()->mutable_stamp()->set_sec(now.sec);
00096   turning_velocities_msg.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
00097 
00098   // Frame ID is not used for this particular message
00099   turning_velocities_msg.mutable_header()->set_frame_id("");
00100 
00101   motor_velocity_reference_pub_->Publish(turning_velocities_msg);
00102 }
00103 
00104 void GazeboControllerInterface::CreatePubsAndSubs() {
00105   gzdbg << __FUNCTION__ << "() called." << std::endl;
00106 
00107   // Create temporary "ConnectGazeboToRosTopic" publisher and message
00108   gazebo::transport::PublisherPtr gz_connect_gazebo_to_ros_topic_pub =
00109       node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00110           "~/" + kConnectGazeboToRosSubtopic, 1);
00111 
00112   // Create temporary "ConnectRosToGazeboTopic" publisher and message
00113   gazebo::transport::PublisherPtr gz_connect_ros_to_gazebo_topic_pub =
00114       node_handle_->Advertise<gz_std_msgs::ConnectRosToGazeboTopic>(
00115           "~/" + kConnectRosToGazeboSubtopic, 1);
00116 
00117   // ============================================================ //
00118   // === ACTUATORS (MOTOR VELOCITY) MSG SETUP (GAZEBO -> ROS) === //
00119   // ============================================================ //
00120 
00121   // TODO This topic is missing the "~" and is in a completely different
00122   // namespace, fix?
00123 
00124   gzdbg << "GazeboControllerInterface creating Gazebo publisher on \""
00125         << namespace_ + "/" + motor_velocity_reference_pub_topic_ << "\"."
00126         << std::endl;
00127   motor_velocity_reference_pub_ =
00128       node_handle_->Advertise<gz_sensor_msgs::Actuators>(
00129           namespace_ + "/" + motor_velocity_reference_pub_topic_, 1);
00130 
00131   // Connect to ROS
00132   gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
00133   connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
00134       namespace_ + "/" + motor_velocity_reference_pub_topic_);
00135   connect_gazebo_to_ros_topic_msg.set_ros_topic(
00136       namespace_ + "/" + motor_velocity_reference_pub_topic_);
00137   connect_gazebo_to_ros_topic_msg.set_msgtype(
00138       gz_std_msgs::ConnectGazeboToRosTopic::ACTUATORS);
00139   gz_connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00140                                               true);
00141 
00142   // ================================================ //
00143   // ===== MOTOR SPEED MSG SETUP (ROS -> GAZEBO) ==== //
00144   // ================================================ //
00145   gzdbg << "Subscribing to Gazebo topic \""
00146         << "~/" + namespace_ + "/" + command_motor_speed_sub_topic_ << "\"."
00147         << std::endl;
00148   cmd_motor_sub_ = node_handle_->Subscribe(
00149       "~/" + namespace_ + "/" + command_motor_speed_sub_topic_,
00150       &GazeboControllerInterface::CommandMotorCallback, this);
00151 
00152   // Connect to ROS
00153   gz_std_msgs::ConnectRosToGazeboTopic connect_ros_to_gazebo_topic_msg;
00154   connect_ros_to_gazebo_topic_msg.set_ros_topic(namespace_ + "/" +
00155                                                 command_motor_speed_sub_topic_);
00156   // connect_ros_to_gazebo_topic_msg.set_gazebo_namespace(namespace_);
00157   connect_ros_to_gazebo_topic_msg.set_gazebo_topic(
00158       "~/" + namespace_ + "/" + command_motor_speed_sub_topic_);
00159   connect_ros_to_gazebo_topic_msg.set_msgtype(
00160       gz_std_msgs::ConnectRosToGazeboTopic::ACTUATORS);
00161   gz_connect_ros_to_gazebo_topic_pub->Publish(connect_ros_to_gazebo_topic_msg,
00162                                               true);
00163 
00164   gzdbg << __FUNCTION__ << "() called." << std::endl;
00165 }
00166 
00167 void GazeboControllerInterface::CommandMotorCallback(
00168     GzActuatorsMsgPtr& actuators_msg) {
00169   if (kPrintOnMsgCallback) {
00170     gzdbg << __FUNCTION__ << "() called." << std::endl;
00171   }
00172 
00173   input_reference_.resize(actuators_msg->angular_velocities_size());
00174   for (int i = 0; i < actuators_msg->angular_velocities_size(); ++i) {
00175     input_reference_[i] = actuators_msg->angular_velocities(i);
00176   }
00177 
00178   // We have received a motor command reference (it may not be the first, but
00179   // this
00180   // does not matter)
00181   received_first_reference_ = true;
00182 }
00183 
00184 GZ_REGISTER_MODEL_PLUGIN(GazeboControllerInterface);
00185 }


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43