00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <cob_gazebo_ros_control/hwi_switch_robot_hw_sim.h>
00020
00021
00022 namespace cob_gazebo_ros_control
00023 {
00024
00025 bool HWISwitchRobotHWSim::initSim(
00026 const std::string& robot_namespace,
00027 ros::NodeHandle model_nh,
00028 gazebo::physics::ModelPtr parent_model,
00029 const urdf::Model *const urdf_model,
00030 std::vector<transmission_interface::TransmissionInfo> transmissions)
00031 {
00032
00033
00034 const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace);
00035
00036
00037 if(enable_joint_filtering_)
00038 {
00039 n_dof_ = enabled_joints_.size();
00040 ROS_INFO_STREAM("JointFiltering is enabled! DoF: "<<n_dof_);
00041 }
00042 else
00043 {
00044 n_dof_ = transmissions.size();
00045 ROS_INFO_STREAM("JointFiltering is disabled! DoF: "<<n_dof_);
00046 }
00047
00048 position_joints_.clear();
00049 position_joints_.insert("test_joint");
00050
00051
00052 joint_names_.resize(n_dof_);
00053 joint_types_.resize(n_dof_);
00054 joint_lower_limits_.resize(n_dof_);
00055 joint_upper_limits_.resize(n_dof_);
00056 joint_effort_limits_.resize(n_dof_);
00057 joint_control_methods_.resize(n_dof_);
00058 map_hwinterface_to_joints_.clear();
00059 map_hwinterface_to_controlmethod_.clear();
00060 joint_position_.resize(n_dof_);
00061 joint_velocity_.resize(n_dof_);
00062 joint_effort_.resize(n_dof_);
00063 joint_effort_command_.resize(n_dof_);
00064 joint_position_command_.resize(n_dof_);
00065 joint_velocity_command_.resize(n_dof_);
00066
00067
00068 unsigned int index = 0;
00069 for(unsigned int j=0; j < transmissions.size(); j++)
00070 {
00071
00072 if(transmissions[j].joints_.size() == 0)
00073 {
00074 ROS_WARN_STREAM_NAMED("hwi_switch_robot_hw_sim","Transmission " << transmissions[j].name_
00075 << " has no associated joints.");
00076 continue;
00077 }
00078 else if(transmissions[j].joints_.size() > 1)
00079 {
00080 ROS_WARN_STREAM_NAMED("hwi_switch_robot_hw_sim","Transmission " << transmissions[j].name_
00081 << " has more than one joint. Currently the default robot hardware simulation "
00082 << " interface only supports one.");
00083 continue;
00084 }
00085
00086 std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
00087 if (joint_interfaces.empty() &&
00088 !(transmissions[j].actuators_.empty()) &&
00089 !(transmissions[j].actuators_[0].hardware_interfaces_.empty()))
00090 {
00091
00092 joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
00093 ROS_WARN_STREAM_NAMED("hwi_switch_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
00094 transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
00095 "The transmission will be properly loaded, but please update " <<
00096 "your robot model to remain compatible with future versions of the plugin.");
00097 }
00098 if (joint_interfaces.empty())
00099 {
00100 ROS_WARN_STREAM_NAMED("hwi_switch_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
00101 " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
00102 "Not adding it to the robot hardware simulation.");
00103 continue;
00104 }
00105 else if (joint_interfaces.size() > 1)
00106 {
00107 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
00108 " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
00109 "This feature is now available.");
00110 }
00111
00112 if(enable_joint_filtering_)
00113 {
00114 if(enabled_joints_.find(transmissions[j].joints_[0].name_)!=enabled_joints_.end())
00115 {
00116 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "Found enabled joint '"<<transmissions[j].joints_[0].name_<<"'; j "<<j<<"; index: "<<index);
00117 }
00118 else
00119 {
00120 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "Joint '"<<transmissions[j].joints_[0].name_<<"' is not enabled; j "<<j<<"; index: "<<index);
00121 continue;
00122 }
00123 }
00124 else
00125 {
00126 index = j;
00127 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "JointFiltering is disabled. Use joint '"<<transmissions[j].joints_[0].name_<<"'; j "<<j<<"; index: "<<index);
00128 }
00129
00130
00131 joint_names_[index] = transmissions[j].joints_[0].name_;
00132 joint_position_[index] = 1.0;
00133 joint_velocity_[index] = 0.0;
00134 joint_effort_[index] = 1.0;
00135 joint_effort_command_[index] = 0.0;
00136 joint_position_command_[index] = 0.0;
00137 joint_velocity_command_[index] = 0.0;
00138
00139
00140
00141 js_interface_.registerHandle(hardware_interface::JointStateHandle(
00142 joint_names_[index], &joint_position_[index], &joint_velocity_[index], &joint_effort_[index]));
00143
00144
00145 hardware_interface::JointHandle joint_handle;
00146
00147
00148 for(unsigned int i=0; i<joint_interfaces.size(); i++)
00149 {
00150
00151 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim","Loading joint '" << joint_names_[index]
00152 << "' of type '" << joint_interfaces[i] << "'");
00153
00154
00155
00156 std::string hw_interface_type = "hardware_interface::"+joint_interfaces[i];
00157 if(map_hwinterface_to_joints_.find(hw_interface_type)!=map_hwinterface_to_joints_.end())
00158 {
00159 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "HW-Interface " << hw_interface_type << " already registered. Adding joint " << joint_names_[index] << " to list.");
00160 std::map< std::string, std::set<std::string> >::iterator it;
00161 it=map_hwinterface_to_joints_.find(hw_interface_type);
00162 it->second.insert(joint_names_[index]);
00163 }
00164 else
00165 {
00166 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "New HW-Interface registered " << hw_interface_type << ". Adding joint " << joint_names_[index] << " to list.");
00167 std::set<std::string> supporting_joints;
00168 supporting_joints.insert(joint_names_[index]);
00169 map_hwinterface_to_joints_.insert( std::pair< std::string, std::set<std::string> >(hw_interface_type, supporting_joints) );
00170 }
00171
00172 if(joint_interfaces[i] == "EffortJointInterface")
00173 {
00174
00175 ControlMethod control_method = EFFORT;
00176 if(i==0){ joint_control_methods_[index] = control_method; }
00177 map_hwinterface_to_controlmethod_.insert( std::pair<std::string, ControlMethod>(hw_interface_type, control_method) );
00178
00179 joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[index]),
00180 &joint_effort_command_[index]);
00181 ej_interface_.registerHandle(joint_handle);
00182
00183 registerJointLimits(joint_names_[index], joint_handle, control_method,
00184 joint_limit_nh, urdf_model,
00185 &joint_types_[index], &joint_lower_limits_[index], &joint_upper_limits_[index],
00186 &joint_effort_limits_[index]);
00187 }
00188 else if(joint_interfaces[i] == "PositionJointInterface")
00189 {
00190
00191 ControlMethod control_method = POSITION;
00192 if(i==0){ joint_control_methods_[index] = control_method; }
00193 map_hwinterface_to_controlmethod_.insert( std::pair<std::string, ControlMethod>(hw_interface_type, control_method) );
00194
00195 joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[index]),
00196 &joint_position_command_[index]);
00197 pj_interface_.registerHandle(joint_handle);
00198
00199 registerJointLimits(joint_names_[index], joint_handle, control_method,
00200 joint_limit_nh, urdf_model,
00201 &joint_types_[index], &joint_lower_limits_[index], &joint_upper_limits_[index],
00202 &joint_effort_limits_[index]);
00203 }
00204 else if(joint_interfaces[i] == "VelocityJointInterface")
00205 {
00206
00207 ControlMethod control_method = VELOCITY;
00208 if(i==0){ joint_control_methods_[index] = control_method; }
00209 map_hwinterface_to_controlmethod_.insert( std::pair<std::string, ControlMethod>(hw_interface_type, control_method) );
00210
00211 joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[index]),
00212 &joint_velocity_command_[index]);
00213 vj_interface_.registerHandle(joint_handle);
00214
00215 registerJointLimits(joint_names_[index], joint_handle, control_method,
00216 joint_limit_nh, urdf_model,
00217 &joint_types_[index], &joint_lower_limits_[index], &joint_upper_limits_[index],
00218 &joint_effort_limits_[index]);
00219 }
00220 else
00221 {
00222 ROS_FATAL_STREAM_NAMED("hwi_switch_robot_hw_sim","No matching hardware interface found for '"
00223 << joint_interfaces[i] );
00224 return false;
00225 }
00226 }
00227
00228 gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[index]);
00229 if (!joint)
00230 {
00231 ROS_ERROR_STREAM_NAMED("hwi_switch_robot_hw_sim", "This robot has a joint named \"" << joint_names_[index]
00232 << "\" which is not in the gazebo model.");
00233 return false;
00234 }
00235 sim_joints_.push_back(joint);
00236
00237
00238
00239 if (joint_control_methods_[index] == VELOCITY || joint_control_methods_[index] == POSITION)
00240 {
00241
00242
00243
00244 joint->SetMaxForce(0, joint_effort_limits_[index]);
00245 }
00246
00247 index++;
00248 }
00249
00250
00251 registerInterface(&js_interface_);
00252 registerInterface(&ej_interface_);
00253 registerInterface(&pj_interface_);
00254 registerInterface(&vj_interface_);
00255
00256
00257 state_valid_ = true;
00258 e_stop_active_ = false;
00259 last_e_stop_active_ = false;
00260
00261 return true;
00262 }
00263
00264 bool HWISwitchRobotHWSim::enableJointFiltering(ros::NodeHandle nh, std::string filter_joints_param)
00265 {
00266 enabled_joints_.clear();
00267 enable_joint_filtering_ = false;
00268
00269 std::vector<std::string> joints;
00270 if(!nh.getParam(filter_joints_param, joints))
00271 {
00272 ROS_ERROR_STREAM_NAMED("hwi_switch_robot_hw_sim", "Parameter '"<<filter_joints_param<<"' not set");
00273 return false;
00274 }
00275
00276 for(std::vector<std::string>::iterator it = joints.begin() ; it != joints.end(); ++it)
00277 {
00278 enabled_joints_.insert(*it);
00279 }
00280
00281 enable_joint_filtering_ = true;
00282 return true;
00283 }
00284
00285 bool HWISwitchRobotHWSim::canSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) const
00286 {
00287
00288
00289
00290 for (std::list<hardware_interface::ControllerInfo>::const_iterator list_it = start_list.begin(); list_it != start_list.end(); ++list_it)
00291 {
00292 for(std::set<std::string>::iterator set_it = list_it->resources.begin(); set_it != list_it->resources.end(); ++set_it)
00293 {
00294 if(map_hwinterface_to_joints_.at(list_it->hardware_interface).find(*set_it) == map_hwinterface_to_joints_.at(list_it->hardware_interface).end())
00295 {
00296 ROS_ERROR_STREAM_NAMED("hwi_switch_robot_hw_sim", "Cannot switch because resource \'" << *set_it << "\' does not provide HW-Interface \'" << list_it->hardware_interface << "\'");
00297 return false;
00298 }
00299 }
00300 }
00301 return true;
00302 }
00303
00304 void HWISwitchRobotHWSim::doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list)
00305 {
00306
00307 for (std::list<hardware_interface::ControllerInfo>::const_iterator list_it = start_list.begin(); list_it != start_list.end(); ++list_it)
00308 {
00309
00310 for(unsigned int i=0; i<joint_names_.size(); i++)
00311 {
00312
00313 if(list_it->resources.find(joint_names_[i]) != list_it->resources.end())
00314 {
00315 if(map_hwinterface_to_controlmethod_.find(list_it->hardware_interface) != map_hwinterface_to_controlmethod_.end())
00316 {
00317 ControlMethod current_control_method = map_hwinterface_to_controlmethod_.find(list_it->hardware_interface)->second;
00318
00320 joint_position_command_[i] = joint_position_[i];
00321 joint_velocity_command_[i] = 0.0;
00322 joint_effort_command_[i] = 0.0;
00323
00325 try{ pj_interface_.getHandle(joint_names_[i]).setCommand(joint_position_command_[i]); }
00326 catch(const hardware_interface::HardwareInterfaceException&){}
00327 try{ vj_interface_.getHandle(joint_names_[i]).setCommand(joint_velocity_command_[i]); }
00328 catch(const hardware_interface::HardwareInterfaceException&){}
00329 try{ ej_interface_.getHandle(joint_names_[i]).setCommand(joint_effort_command_[i]); }
00330 catch(const hardware_interface::HardwareInterfaceException&){}
00331
00333 pj_sat_interface_.reset();
00334 pj_limits_interface_.reset();
00335
00336 joint_control_methods_[i] = current_control_method;
00337
00338 ROS_DEBUG_STREAM_NAMED("hwi_switch_robot_hw_sim", "Resource \'" << joint_names_[i] << "\' switched to HW-Interface \'" << list_it->hardware_interface << "\'");
00339 }
00340 }
00341 }
00342 }
00343 }
00344
00345 void HWISwitchRobotHWSim::stateValid(const bool valid)
00346 {
00347 state_valid_ = valid;
00348 }
00349
00350 }
00351
00352 PLUGINLIB_EXPORT_CLASS(cob_gazebo_ros_control::HWISwitchRobotHWSim, gazebo_ros_control::RobotHWSim)