00001 #include "epos_hardware/epos.h"
00002 #include <boost/foreach.hpp>
00003
00004 namespace epos_hardware {
00005
00006 Epos::Epos(const std::string& name,
00007 ros::NodeHandle& nh, ros::NodeHandle& config_nh,
00008 EposFactory* epos_factory,
00009 hardware_interface::ActuatorStateInterface& asi,
00010 hardware_interface::VelocityActuatorInterface& avi,
00011 hardware_interface::PositionActuatorInterface& api)
00012 : name_(name), config_nh_(config_nh), diagnostic_updater_(nh, config_nh), epos_factory_(epos_factory),
00013 has_init_(false),
00014 position_(0), velocity_(0), effort_(0), current_(0), statusword_(0),
00015 position_cmd_(0), velocity_cmd_(0) {
00016
00017 valid_ = true;
00018 if(!config_nh_.getParam("actuator_name", actuator_name_)) {
00019 ROS_ERROR("You must specify an actuator name");
00020 valid_ = false;
00021 }
00022
00023 std::string serial_number_str;
00024 if(!config_nh_.getParam("serial_number", serial_number_str)) {
00025 ROS_ERROR("You must specify a serial number");
00026 valid_ = false;
00027 }
00028 else {
00029 ROS_ASSERT(SerialNumberFromHex(serial_number_str, &serial_number_));
00030 }
00031
00032
00033 std::string operation_mode_str;
00034 if(!config_nh_.getParam("operation_mode", operation_mode_str)) {
00035 ROS_ERROR("You must specify an operation mode");
00036 valid_ = false;
00037 }
00038 else {
00039 if(operation_mode_str == "profile_position") {
00040 operation_mode_ = PROFILE_POSITION_MODE;
00041 }
00042 else if(operation_mode_str == "profile_velocity") {
00043 operation_mode_ = PROFILE_VELOCITY_MODE;
00044 }
00045 else {
00046 ROS_ERROR_STREAM(operation_mode_str << " is not a valid operation mode");
00047 valid_ = false;
00048 }
00049 }
00050
00051 ROS_INFO_STREAM(actuator_name_);
00052 hardware_interface::ActuatorStateHandle state_handle(actuator_name_, &position_, &velocity_, &effort_);
00053 asi.registerHandle(state_handle);
00054
00055 hardware_interface::ActuatorHandle position_handle(state_handle, &position_cmd_);
00056 api.registerHandle(position_handle);
00057 hardware_interface::ActuatorHandle velocity_handle(state_handle, &velocity_cmd_);
00058 avi.registerHandle(velocity_handle);
00059
00060 diagnostic_updater_.setHardwareID(serial_number_str);
00061 std::stringstream motor_diagnostic_name_ss;
00062 motor_diagnostic_name_ss << name << ": " << "Motor";
00063 diagnostic_updater_.add(motor_diagnostic_name_ss.str(), boost::bind(&Epos::buildMotorStatus, this, _1));
00064 std::stringstream motor_output_diagnostic_name_ss;
00065 motor_output_diagnostic_name_ss << name << ": " << "Motor Output";
00066 diagnostic_updater_.add(motor_output_diagnostic_name_ss.str(), boost::bind(&Epos::buildMotorOutputStatus, this, _1));
00067 }
00068
00069 Epos::~Epos() {
00070 unsigned int error_code;
00071 if(node_handle_)
00072 VCS_SetDisableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code);
00073 }
00074
00075 class ParameterSetLoader {
00076 public:
00077 ParameterSetLoader(ros::NodeHandle nh) : nh_(nh){}
00078 ParameterSetLoader(ros::NodeHandle parent_nh, const std::string& name) : nh_(parent_nh, name){}
00079 template <class T> ParameterSetLoader& param(const std::string& name, T& value) {
00080 if(nh_.getParam(name, value))
00081 found_.push_back(name);
00082 else
00083 not_found_.push_back(name);
00084 return *this;
00085 }
00086 bool all_or_none(bool& found_all) {
00087 if(not_found_.size() == 0) {
00088 found_all = true;
00089 return true;
00090 }
00091 if(found_.size() == 0) {
00092 found_all = false;
00093 return true;
00094 }
00095 ROS_ERROR_STREAM("Expected all or none parameter set: (" << nh_.getNamespace() << ")");
00096 BOOST_FOREACH(const std::string& name, found_) {
00097 ROS_ERROR_STREAM("\tFound: " << nh_.resolveName(name));
00098 }
00099 BOOST_FOREACH(const std::string& name, not_found_) {
00100 ROS_ERROR_STREAM("\tExpected: " << nh_.resolveName(name));
00101 }
00102 return false;
00103 }
00104
00105 private:
00106 ros::NodeHandle nh_;
00107 std::vector<std::string> found_;
00108 std::vector<std::string> not_found_;
00109 };
00110
00111 #define VCS(func, ...) \
00112 if(!VCS_##func(node_handle_->device_handle->ptr, node_handle_->node_id, __VA_ARGS__, &error_code)) { \
00113 ROS_ERROR("Failed to "#func); \
00114 return false; \
00115 }
00116
00117 #define VCS_FROM_SINGLE_PARAM_REQUIRED(nh, type, name, func) \
00118 type name; \
00119 if(!nh.getParam(#name, name)) { \
00120 ROS_ERROR_STREAM(nh.resolveName(#name) << " not specified"); \
00121 return false; \
00122 } \
00123 else { \
00124 VCS(func, name); \
00125 }
00126 #define VCS_FROM_SINGLE_PARAM_OPTIONAL(nh, type, name, func) \
00127 bool name##_set; \
00128 type name; \
00129 if(name##_set = nh.getParam(#name, name)) { \
00130 VCS(func, name); \
00131 } \
00132
00133
00134 bool Epos::init() {
00135 if(!valid_) {
00136 ROS_ERROR_STREAM("Not Initializing: 0x" << std::hex << serial_number_ << ", initial construction failed");
00137 return false;
00138 }
00139
00140 ROS_INFO_STREAM("Initializing: 0x" << std::hex << serial_number_);
00141 unsigned int error_code;
00142 node_handle_ = epos_factory_->CreateNodeHandle("EPOS2", "MAXON SERIAL V2", "USB", serial_number_, &error_code);
00143 if(!node_handle_) {
00144 ROS_ERROR("Could not find motor");
00145 return false;
00146 }
00147 ROS_INFO_STREAM("Found Motor");
00148
00149 if(!VCS_SetProtocolStackSettings(node_handle_->device_handle->ptr, 1000000, 500, &error_code)) {
00150 ROS_ERROR("Failed to SetProtocolStackSettings");
00151 return false;
00152 }
00153
00154 if(!VCS_SetDisableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) {
00155 ROS_ERROR("Failed to SetDisableState");
00156 return false;
00157 }
00158
00159 VCS(SetOperationMode, operation_mode_);
00160
00161 std::string fault_reaction_str;
00162 #define SET_FAULT_REACTION_OPTION(val) \
00163 do { \
00164 unsigned int length = 2; \
00165 unsigned int bytes_written; \
00166 int16_t data = val; \
00167 VCS(SetObject, 0x605E, 0x00, &data, length, &bytes_written); \
00168 } while(true)
00169
00170 if(config_nh_.getParam("fault_reaction_option", fault_reaction_str)) {
00171 if(fault_reaction_str == "signal_only") {
00172 SET_FAULT_REACTION_OPTION(-1);
00173 }
00174 else if(fault_reaction_str == "disable_drive") {
00175 SET_FAULT_REACTION_OPTION(0);
00176 }
00177 else if(fault_reaction_str == "slow_down_ramp") {
00178 SET_FAULT_REACTION_OPTION(1);
00179 }
00180 else if(fault_reaction_str == "slow_down_quickstop") {
00181 SET_FAULT_REACTION_OPTION(2);
00182 }
00183 else {
00184 ROS_ERROR_STREAM(fault_reaction_str << " is not a valid fault reaction option");
00185 return false;
00186 }
00187 }
00188
00189
00190 ROS_INFO("Configuring Motor");
00191 {
00192 nominal_current_ = 0;
00193 max_current_ = 0;
00194 ros::NodeHandle motor_nh(config_nh_, "motor");
00195
00196 VCS_FROM_SINGLE_PARAM_REQUIRED(motor_nh, int, type, SetMotorType);
00197
00198 {
00199 bool dc_motor;
00200 double nominal_current;
00201 double max_output_current;
00202 double thermal_time_constant;
00203 if(!ParameterSetLoader(motor_nh, "dc_motor")
00204 .param("nominal_current", nominal_current)
00205 .param("max_output_current", max_output_current)
00206 .param("thermal_time_constant", thermal_time_constant)
00207 .all_or_none(dc_motor))
00208 return false;
00209 if(dc_motor){
00210 nominal_current_ = nominal_current;
00211 max_current_ = max_output_current;
00212 VCS(SetDcMotorParameter,
00213 (int)(1000 * nominal_current),
00214 (int)(1000 * max_output_current),
00215 (int)(10 * thermal_time_constant)
00216 );
00217 }
00218 }
00219
00220
00221 {
00222 bool ec_motor;
00223 double nominal_current;
00224 double max_output_current;
00225 double thermal_time_constant;
00226 int number_of_pole_pairs;
00227 if(!ParameterSetLoader(motor_nh, "ec_motor")
00228 .param("nominal_current", nominal_current)
00229 .param("max_output_current", max_output_current)
00230 .param("thermal_time_constant", thermal_time_constant)
00231 .param("number_of_pole_pairs", number_of_pole_pairs)
00232 .all_or_none(ec_motor))
00233 return false;
00234
00235 if(ec_motor) {
00236 nominal_current_ = nominal_current;
00237 max_current_ = max_output_current;
00238 VCS(SetEcMotorParameter,
00239 (int)(1000 * nominal_current),
00240 (int)(1000 * max_output_current),
00241 (int)(10 * thermal_time_constant),
00242 number_of_pole_pairs);
00243 }
00244 }
00245 }
00246
00247 ROS_INFO("Configuring Sensor");
00248 {
00249 ros::NodeHandle sensor_nh(config_nh_, "sensor");
00250
00251 VCS_FROM_SINGLE_PARAM_REQUIRED(sensor_nh, int, type, SetSensorType);
00252
00253 {
00254 bool incremental_encoder;
00255 int resolution;
00256 bool inverted_polarity;
00257
00258 if(!ParameterSetLoader(sensor_nh, "incremental_encoder")
00259 .param("resolution", resolution)
00260 .param("inverted_polarity", inverted_polarity)
00261 .all_or_none(incremental_encoder))
00262 return false;
00263 if(incremental_encoder) {
00264 VCS(SetIncEncoderParameter, resolution, inverted_polarity);
00265 }
00266 }
00267
00268 {
00269 bool hall_sensor;
00270 bool inverted_polarity;
00271
00272 if(!ParameterSetLoader(sensor_nh, "hall_sensor")
00273 .param("inverted_polarity", inverted_polarity)
00274 .all_or_none(hall_sensor))
00275 return false;
00276 if(hall_sensor) {
00277 VCS(SetHallSensorParameter, inverted_polarity);
00278 }
00279 }
00280
00281 {
00282 bool ssi_absolute_encoder;
00283 int data_rate;
00284 int number_of_multiturn_bits;
00285 int number_of_singleturn_bits;
00286 bool inverted_polarity;
00287
00288 if(!ParameterSetLoader(sensor_nh, "ssi_absolute_encoder")
00289 .param("data_rate", data_rate)
00290 .param("number_of_multiturn_bits", number_of_multiturn_bits)
00291 .param("number_of_singleturn_bits", number_of_singleturn_bits)
00292 .param("inverted_polarity", inverted_polarity)
00293 .all_or_none(ssi_absolute_encoder))
00294 return false;
00295 if(ssi_absolute_encoder) {
00296 VCS(SetSsiAbsEncoderParameter,
00297 data_rate,
00298 number_of_multiturn_bits,
00299 number_of_singleturn_bits,
00300 inverted_polarity);
00301 }
00302 }
00303
00304 }
00305
00306 {
00307 ROS_INFO("Configuring Safety");
00308 ros::NodeHandle safety_nh(config_nh_, "safety");
00309
00310 VCS_FROM_SINGLE_PARAM_OPTIONAL(safety_nh, int, max_following_error, SetMaxFollowingError);
00311 VCS_FROM_SINGLE_PARAM_OPTIONAL(safety_nh, int, max_profile_velocity, SetMaxProfileVelocity);
00312 VCS_FROM_SINGLE_PARAM_OPTIONAL(safety_nh, int, max_acceleration, SetMaxAcceleration);
00313 if(max_profile_velocity_set)
00314 max_profile_velocity_ = max_profile_velocity;
00315 else
00316 max_profile_velocity_ = -1;
00317 }
00318
00319 {
00320 ROS_INFO("Configuring Position Regulator");
00321 ros::NodeHandle position_regulator_nh(config_nh_, "position_regulator");
00322 {
00323 bool position_regulator_gain;
00324 int p, i, d;
00325 if(!ParameterSetLoader(position_regulator_nh, "gain")
00326 .param("p", p)
00327 .param("i", i)
00328 .param("d", d)
00329 .all_or_none(position_regulator_gain))
00330 return false;
00331 if(position_regulator_gain){
00332 VCS(SetPositionRegulatorGain, p, i, d);
00333 }
00334 }
00335
00336 {
00337 bool position_regulator_feed_forward;
00338 int velocity, acceleration;
00339 if(!ParameterSetLoader(position_regulator_nh, "feed_forward")
00340 .param("velocity", velocity)
00341 .param("acceleration", acceleration)
00342 .all_or_none(position_regulator_feed_forward))
00343 return false;
00344 if(position_regulator_feed_forward){
00345 VCS(SetPositionRegulatorFeedForward, velocity, acceleration);
00346 }
00347 }
00348 }
00349
00350 {
00351 ROS_INFO("Configuring Velocity Regulator");
00352 ros::NodeHandle velocity_regulator_nh(config_nh_, "velocity_regulator");
00353 {
00354 bool velocity_regulator_gain;
00355 int p, i;
00356 if(!ParameterSetLoader(velocity_regulator_nh, "gain")
00357 .param("p", p)
00358 .param("i", i)
00359 .all_or_none(velocity_regulator_gain))
00360 return false;
00361 if(velocity_regulator_gain){
00362 VCS(SetVelocityRegulatorGain, p, i);
00363 }
00364 }
00365
00366 {
00367 bool velocity_regulator_feed_forward;
00368 int velocity, acceleration;
00369 if(!ParameterSetLoader(velocity_regulator_nh, "feed_forward")
00370 .param("velocity", velocity)
00371 .param("acceleration", acceleration)
00372 .all_or_none(velocity_regulator_feed_forward))
00373 return false;
00374 if(velocity_regulator_feed_forward){
00375 VCS(SetVelocityRegulatorFeedForward, velocity, acceleration);
00376 }
00377 }
00378 }
00379
00380
00381 {
00382 ROS_INFO("Configuring Current Regulator");
00383 ros::NodeHandle current_regulator_nh(config_nh_, "current_regulator");
00384 {
00385 bool current_regulator_gain;
00386 int p, i;
00387 if(!ParameterSetLoader(current_regulator_nh, "gain")
00388 .param("p", p)
00389 .param("i", i)
00390 .all_or_none(current_regulator_gain))
00391 return false;
00392 if(current_regulator_gain){
00393 VCS(SetCurrentRegulatorGain, p, i);
00394 }
00395 }
00396 }
00397
00398
00399 {
00400 ROS_INFO("Configuring Position Profile");
00401 ros::NodeHandle position_profile_nh(config_nh_, "position_profile");
00402 {
00403 bool position_profile;
00404 int velocity, acceleration, deceleration;
00405 if(!ParameterSetLoader(position_profile_nh)
00406 .param("velocity", velocity)
00407 .param("acceleration", acceleration)
00408 .param("deceleration", deceleration)
00409 .all_or_none(position_profile))
00410 return false;
00411 if(position_profile){
00412 VCS(SetPositionProfile, velocity, acceleration, deceleration);
00413 }
00414 }
00415
00416 {
00417 bool position_profile_window;
00418 int window;
00419 double time;
00420 if(!ParameterSetLoader(position_profile_nh, "window")
00421 .param("window", window)
00422 .param("time", time)
00423 .all_or_none(position_profile_window))
00424 return false;
00425 if(position_profile_window){
00426 VCS(EnablePositionWindow,
00427 window,
00428 (int)(1000 * time)
00429 );
00430 }
00431 }
00432 }
00433
00434 {
00435 ROS_INFO("Configuring Velocity Profile");
00436 ros::NodeHandle velocity_profile_nh(config_nh_, "velocity_profile");
00437 {
00438 bool velocity_profile;
00439 int acceleration, deceleration;
00440 if(!ParameterSetLoader(velocity_profile_nh)
00441 .param("acceleration", acceleration)
00442 .param("deceleration", deceleration)
00443 .all_or_none(velocity_profile))
00444 return false;
00445 if(velocity_profile){
00446 VCS(SetVelocityProfile, acceleration, deceleration);
00447 }
00448 }
00449
00450 {
00451 bool velocity_profile_window;
00452 int window;
00453 double time;
00454 if(!ParameterSetLoader(velocity_profile_nh, "window")
00455 .param("window", window)
00456 .param("time", time)
00457 .all_or_none(velocity_profile_window))
00458 return false;
00459 if(velocity_profile_window){
00460 VCS(EnableVelocityWindow,
00461 window,
00462 (int)(1000 * time)
00463 );
00464 }
00465 }
00466 }
00467
00468
00469
00470 ROS_INFO("Querying Faults");
00471 unsigned char num_errors;
00472 if(!VCS_GetNbOfDeviceError(node_handle_->device_handle->ptr, node_handle_->node_id, &num_errors, &error_code))
00473 return false;
00474 for(int i = 1; i<= num_errors; ++i) {
00475 unsigned int error_number;
00476 if(!VCS_GetDeviceErrorCode(node_handle_->device_handle->ptr, node_handle_->node_id, i, &error_number, &error_code))
00477 return false;
00478 ROS_WARN_STREAM("EPOS Device Error: 0x" << std::hex << error_number);
00479 }
00480
00481 bool clear_faults;
00482 config_nh_.param<bool>("clear_faults", clear_faults, false);
00483 if(num_errors > 0) {
00484 if(clear_faults) {
00485 ROS_INFO("Clearing faults");
00486 if(!VCS_ClearFault(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) {
00487 ROS_ERROR("Could not clear faults");
00488 return false;
00489 }
00490 else
00491 ROS_INFO("Cleared faults");
00492 }
00493 else {
00494 ROS_ERROR("Not clearing faults, but faults exist");
00495 return false;
00496 }
00497 }
00498
00499 if(!VCS_GetNbOfDeviceError(node_handle_->device_handle->ptr, node_handle_->node_id, &num_errors, &error_code))
00500 return false;
00501 if(num_errors > 0) {
00502 ROS_ERROR("Not all faults were cleared");
00503 return false;
00504 }
00505
00506 config_nh_.param<bool>("halt_velocity", halt_velocity_, false);
00507
00508 if(!config_nh_.getParam("torque_constant", torque_constant_)) {
00509 ROS_WARN("No torque constant specified, you can supply one using the 'torque_constant' parameter");
00510 torque_constant_ = 1.0;
00511 }
00512
00513 ROS_INFO_STREAM("Enabling Motor");
00514 if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code))
00515 return false;
00516
00517 has_init_ = true;
00518 return true;
00519 }
00520
00521 void Epos::read() {
00522 if(!has_init_)
00523 return;
00524
00525 unsigned int error_code;
00526
00527
00528 unsigned int bytes_read;
00529 VCS_GetObject(node_handle_->device_handle->ptr, node_handle_->node_id, 0x6041, 0x00, &statusword_, 2, &bytes_read, &error_code);
00530
00531 int position_raw;
00532 int velocity_raw;
00533 short current_raw;
00534 VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code);
00535 VCS_GetVelocityIs(node_handle_->device_handle->ptr, node_handle_->node_id, &velocity_raw, &error_code);
00536 VCS_GetCurrentIs(node_handle_->device_handle->ptr, node_handle_->node_id, ¤t_raw, &error_code);
00537 position_ = position_raw;
00538 velocity_ = velocity_raw;
00539 current_ = current_raw / 1000.0;
00540 effort_ = current_ * torque_constant_;
00541
00542 }
00543
00544 void Epos::write() {
00545 if(!has_init_)
00546 return;
00547
00548 unsigned int error_code;
00549 if(operation_mode_ == PROFILE_VELOCITY_MODE) {
00550 if(isnan(velocity_cmd_))
00551 return;
00552 int cmd = (int)velocity_cmd_;
00553 if(max_profile_velocity_ >= 0) {
00554 if(cmd < -max_profile_velocity_)
00555 cmd = -max_profile_velocity_;
00556 if(cmd > max_profile_velocity_)
00557 cmd = max_profile_velocity_;
00558 }
00559
00560 if(cmd == 0 && halt_velocity_) {
00561 VCS_HaltVelocityMovement(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code);
00562 }
00563 else {
00564 VCS_MoveWithVelocity(node_handle_->device_handle->ptr, node_handle_->node_id, cmd, &error_code);
00565 }
00566 }
00567 else if(operation_mode_ == PROFILE_POSITION_MODE) {
00568 if(isnan(position_cmd_))
00569 return;
00570 VCS_MoveToPosition(node_handle_->device_handle->ptr, node_handle_->node_id, (int)position_cmd_, true, true, &error_code);
00571 }
00572 }
00573
00574 void Epos::update_diagnostics() {
00575 diagnostic_updater_.update();
00576 }
00577 void Epos::buildMotorStatus(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00578 stat.add("Actuator Name", actuator_name_);
00579 unsigned int error_code;
00580 if(has_init_) {
00581 bool enabled = STATUSWORD(READY_TO_SWITCH_ON, statusword_) && STATUSWORD(SWITCHED_ON, statusword_) && STATUSWORD(ENABLE, statusword_);
00582 if(enabled) {
00583 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Enabled");
00584 }
00585 else {
00586 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Disabled");
00587 }
00588
00589
00590 if(!STATUSWORD(QUICKSTOP, statusword_) && enabled) {
00591 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Quickstop");
00592 }
00593
00594 if(STATUSWORD(WARNING, statusword_)) {
00595 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Warning");
00596 }
00597
00598 if(STATUSWORD(FAULT, statusword_)) {
00599 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Fault");
00600 }
00601
00602 stat.add<bool>("Enabled", STATUSWORD(ENABLE, statusword_));
00603 stat.add<bool>("Fault", STATUSWORD(FAULT, statusword_));
00604 stat.add<bool>("Voltage Enabled", STATUSWORD(VOLTAGE_ENABLED, statusword_));
00605 stat.add<bool>("Quickstop", STATUSWORD(QUICKSTOP, statusword_));
00606 stat.add<bool>("Warning", STATUSWORD(WARNING, statusword_));
00607
00608 unsigned char num_errors;
00609 if(VCS_GetNbOfDeviceError(node_handle_->device_handle->ptr, node_handle_->node_id, &num_errors, &error_code)) {
00610 for(int i = 1; i<= num_errors; ++i) {
00611 unsigned int error_number;
00612 if(VCS_GetDeviceErrorCode(node_handle_->device_handle->ptr, node_handle_->node_id, i, &error_number, &error_code)) {
00613 std::stringstream error_msg;
00614 error_msg << "EPOS Device Error: 0x" << std::hex << error_number;
00615 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, error_msg.str());
00616 }
00617 else {
00618 std::string error_str;
00619 if(GetErrorInfo(error_code, &error_str)) {
00620 std::stringstream error_msg;
00621 error_msg << "Could not read device error: " << error_str;
00622 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, error_msg.str());
00623 }
00624 else {
00625 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not read device error");
00626 }
00627 }
00628 }
00629 }
00630 else {
00631 std::string error_str;
00632 if(GetErrorInfo(error_code, &error_str)) {
00633 std::stringstream error_msg;
00634 error_msg << "Could not read device errors: " << error_str;
00635 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, error_msg.str());
00636 }
00637 else {
00638 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not read device errors");
00639 }
00640 }
00641
00642
00643 }
00644 else {
00645 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "EPOS not initialized");
00646 }
00647 }
00648
00649 void Epos::buildMotorOutputStatus(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00650 std::string operation_mode_str;
00651 if(operation_mode_ == PROFILE_POSITION_MODE) {
00652 operation_mode_str = "Profile Position Mode";
00653 stat.add("Commanded Position", boost::lexical_cast<std::string>(position_cmd_) + " rotations");
00654 }
00655 else if(operation_mode_ == PROFILE_VELOCITY_MODE) {
00656 operation_mode_str = "Profile Velocity Mode";
00657 stat.add("Commanded Velocity", boost::lexical_cast<std::string>(velocity_cmd_) + " rpm");
00658 }
00659 else {
00660 operation_mode_str = "Unknown Mode";
00661 }
00662 stat.add("Operation Mode", operation_mode_str);
00663 stat.add("Nominal Current", boost::lexical_cast<std::string>(nominal_current_) + " A");
00664 stat.add("Max Current", boost::lexical_cast<std::string>(max_current_) + " A");
00665
00666 unsigned int error_code;
00667 if(has_init_) {
00668 stat.add("Position", boost::lexical_cast<std::string>(position_) + " rotations");
00669 stat.add("Velocity", boost::lexical_cast<std::string>(velocity_) + " rpm");
00670 stat.add("Torque", boost::lexical_cast<std::string>(effort_) + " Nm");
00671 stat.add("Current", boost::lexical_cast<std::string>(current_) + " A");
00672
00673
00674 stat.add<bool>("Target Reached", STATUSWORD(TARGET_REACHED, statusword_));
00675 stat.add<bool>("Current Limit Active", STATUSWORD(CURRENT_LIMIT_ACTIVE, statusword_));
00676
00677
00678 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "EPOS operating in " + operation_mode_str);
00679 if(STATUSWORD(CURRENT_LIMIT_ACTIVE, statusword_))
00680 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Current Limit Active");
00681 if(nominal_current_ > 0 && std::abs(current_) > nominal_current_) {
00682 stat.mergeSummaryf(diagnostic_msgs::DiagnosticStatus::WARN, "Nominal Current Exceeded (Current: %f A)", current_);
00683 }
00684
00685
00686 }
00687 else {
00688 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "EPOS not initialized");
00689 }
00690 }
00691
00692
00693 }