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 #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
00039 model_ = _model;
00040
00041 world_ = model_->GetWorld();
00042
00043 namespace_.clear();
00044
00045
00046
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
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
00068
00069 updateConnection_ = event::Events::ConnectWorldUpdateBegin(
00070 boost::bind(&GazeboControllerInterface::OnUpdate, this, _1));
00071 }
00072
00073 void GazeboControllerInterface::OnUpdate(const common::UpdateInfo& ) {
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
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
00108 gazebo::transport::PublisherPtr gz_connect_gazebo_to_ros_topic_pub =
00109 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00110 "~/" + kConnectGazeboToRosSubtopic, 1);
00111
00112
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
00119
00120
00121
00122
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
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
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
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
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
00179
00180
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