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 #include <cob_gazebo_ros_control/hwi_switch_gazebo_ros_control_plugin.h>
00019 #include <urdf/model.h>
00020
00021 namespace cob_gazebo_ros_control
00022 {
00023
00024
00025 void HWISwitchGazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
00026 {
00027 ROS_INFO_STREAM_NAMED("cob_gazebo_ros_control","Loading cob_gazebo_ros_control plugin");
00028 enable_joint_filtering_ = false;
00029
00030
00031 parent_model_ = parent;
00032 sdf_ = sdf;
00033
00034
00035 if (!parent_model_)
00036 {
00037 ROS_ERROR_STREAM_NAMED("cob_gazebo_ros_control","parent model is NULL");
00038 return;
00039 }
00040
00041
00042 if(!ros::isInitialized())
00043 {
00044 ROS_FATAL_STREAM_NAMED("cob_gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. "
00045 << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00046 return;
00047 }
00048
00049
00050 if(sdf_->HasElement("robotNamespace"))
00051 {
00052 robot_namespace_ = sdf_->GetElement("robotNamespace")->Get<std::string>();
00053 }
00054 else
00055 {
00056 robot_namespace_ = parent_model_->GetName();
00057 }
00058
00059
00060 if (sdf_->HasElement("robotParam"))
00061 {
00062 robot_description_ = sdf_->GetElement("robotParam")->Get<std::string>();
00063 }
00064 else
00065 {
00066 robot_description_ = "robot_description";
00067 }
00068
00069
00070 if(sdf_->HasElement("robotSimType"))
00071 {
00072
00073 robot_hw_sim_type_str_ = "cob_gazebo_ros_control/HWISwitchRobotHWSim";
00074 ROS_WARN_STREAM_NAMED("cob_gazebo_ros_control","Tag 'robotSimType' is currently ignored. Using default plugin for RobotHWSim \""<<robot_hw_sim_type_str_<<"\"");
00075 }
00076 else
00077 {
00078 robot_hw_sim_type_str_ = "cob_gazebo_ros_control/HWISwitchRobotHWSim";
00079 ROS_DEBUG_STREAM_NAMED("cob_gazebo_ros_control","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
00080 }
00081
00082
00083 ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize());
00084
00085
00086 if(sdf_->HasElement("controlPeriod"))
00087 {
00088 control_period_ = ros::Duration(sdf_->Get<double>("controlPeriod"));
00089
00090
00091 if( control_period_ < gazebo_period )
00092 {
00093 ROS_ERROR_STREAM_NAMED("cob_gazebo_ros_control","Desired controller update period ("<<control_period_
00094 <<" s) is faster than the gazebo simulation period ("<<gazebo_period<<" s).");
00095 }
00096 else if( control_period_ > gazebo_period )
00097 {
00098 ROS_WARN_STREAM_NAMED("cob_gazebo_ros_control","Desired controller update period ("<<control_period_
00099 <<" s) is slower than the gazebo simulation period ("<<gazebo_period<<" s).");
00100 }
00101 }
00102 else
00103 {
00104 control_period_ = gazebo_period;
00105 ROS_DEBUG_STREAM_NAMED("cob_gazebo_ros_control","Control period not found in URDF/SDF, defaulting to Gazebo period of "
00106 << control_period_);
00107 }
00108
00109
00110 model_nh_ = ros::NodeHandle(robot_namespace_);
00111
00112
00113 e_stop_active_ = false;
00114 last_e_stop_active_ = false;
00115 if (sdf_->HasElement("eStopTopic"))
00116 {
00117 const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get<std::string>();
00118 e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &HWISwitchGazeboRosControlPlugin::eStopCB, this);
00119 }
00120
00121
00122 state_valid_ = true;
00123 if (sdf_->HasElement("stateValidTopic"))
00124 {
00125 const std::string state_valid_topic = sdf_->GetElement("stateValidTopic")->Get<std::string>();
00126 state_valid_sub_ = model_nh_.subscribe(state_valid_topic, 1, &HWISwitchGazeboRosControlPlugin::stateValidCB, this);
00127 }
00128
00129
00130 if(sdf_->HasElement("filterJointsParam"))
00131 {
00132 enable_joint_filtering_ = true;
00133 filterJointsParam_ = sdf_->Get<std::string>("filterJointsParam");
00134 ROS_INFO_STREAM_NAMED("cob_gazebo_ros_control","Enable joint-filtering. Using joint_names from parameter: \""<<filterJointsParam_<<"\"");
00135 }
00136 else
00137 {
00138 enable_joint_filtering_ = false;
00139 filterJointsParam_ = "";
00140 ROS_INFO_STREAM_NAMED("cob_gazebo_ros_control","Joint-filtering is disabled. The plugin will set up JointHandles for all joints!");
00141 }
00142
00143 ROS_INFO_NAMED("cob_gazebo_ros_control", "Starting cob_gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str());
00144
00145
00146
00147
00148 const std::string urdf_string = getURDF(robot_description_);
00149 if (!parseTransmissionsFromURDF(urdf_string))
00150 {
00151 ROS_ERROR_NAMED("cob_gazebo_ros_control", "Error parsing URDF in cob_gazebo_ros_control plugin, plugin not active.\n");
00152 return;
00153 }
00154
00155
00156 try
00157 {
00158
00159
00160
00161
00162
00163
00164
00165 hwi_switch_robot_hw_sim_.reset(new cob_gazebo_ros_control::HWISwitchRobotHWSim());
00166
00167
00168 urdf::Model urdf_model;
00169 const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
00170
00171 if(enable_joint_filtering_)
00172 {
00173 if(!hwi_switch_robot_hw_sim_->enableJointFiltering(model_nh_, filterJointsParam_))
00174 {
00175 ROS_FATAL_STREAM_NAMED("cob_gazebo_ros_control","Joint-filtering was enabled but param '"<<filterJointsParam_<<"' was not found");
00176 return;
00177 }
00178 }
00179
00180 if(!hwi_switch_robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_))
00181 {
00182 ROS_FATAL_NAMED("cob_gazebo_ros_control","Could not initialize robot simulation interface");
00183 return;
00184 }
00185
00186
00187 ROS_DEBUG_STREAM_NAMED("cob_gazebo_ros_control","Loading controller_manager");
00188 controller_manager_.reset
00189 (new controller_manager::ControllerManager(hwi_switch_robot_hw_sim_.get(), model_nh_));
00190
00191
00192 update_connection_ =
00193 gazebo::event::Events::ConnectWorldUpdateBegin
00194 (boost::bind(&HWISwitchGazeboRosControlPlugin::Update, this));
00195
00196 }
00197 catch(pluginlib::LibraryLoadException &ex)
00198 {
00199 ROS_FATAL_STREAM_NAMED("cob_gazebo_ros_control","Failed to create robot simulation interface loader: "<<ex.what());
00200 }
00201
00202 ROS_INFO_NAMED("cob_gazebo_ros_control", "Loaded cob_gazebo_ros_control.");
00203 }
00204
00205
00206 void HWISwitchGazeboRosControlPlugin::Update()
00207 {
00208
00209 gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
00210 ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
00211 ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
00212
00213 hwi_switch_robot_hw_sim_->eStopActive(e_stop_active_);
00214 hwi_switch_robot_hw_sim_->stateValid(state_valid_);
00215 e_stop_active_ = e_stop_active_ || not state_valid_;
00216
00217
00218 if(sim_period >= control_period_) {
00219
00220 last_update_sim_time_ros_ = sim_time_ros;
00221
00222
00223 hwi_switch_robot_hw_sim_->readSim(sim_time_ros, sim_period);
00224
00225
00226 bool reset_ctrlrs;
00227 if (e_stop_active_)
00228 {
00229 reset_ctrlrs = false;
00230 last_e_stop_active_ = true;
00231 }
00232 else
00233 {
00234 if (last_e_stop_active_)
00235 {
00236 reset_ctrlrs = true;
00237 last_e_stop_active_ = false;
00238 }
00239 else
00240 {
00241 reset_ctrlrs = false;
00242 }
00243 }
00244 controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
00245 }
00246
00247
00248
00249 hwi_switch_robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
00250 last_write_sim_time_ros_ = sim_time_ros;
00251 }
00252
00253
00254 void HWISwitchGazeboRosControlPlugin::eStopCB(const std_msgs::BoolConstPtr& e_stop_active)
00255 {
00256 e_stop_active_ = e_stop_active->data;
00257 }
00258
00259
00260 void HWISwitchGazeboRosControlPlugin::stateValidCB(const std_msgs::BoolConstPtr& state_valid)
00261 {
00262 state_valid_ = state_valid->data;
00263 }
00264
00265
00266 GZ_REGISTER_MODEL_PLUGIN(HWISwitchGazeboRosControlPlugin);
00267 }