serial_proxy.cpp
Go to the documentation of this file.
00001 
00002 // Author: Antons Rebguns
00003 
00004 #include <stdint.h>
00005 #include <time.h>
00006 
00007 #include <cmath>
00008 #include <sstream>
00009 #include <string>
00010 #include <vector>
00011 
00012 #include <boost/thread.hpp>
00013 #include <XmlRpcValue.h>
00014 
00015 #include <gearbox/flexiport/flexiport.h>
00016 
00017 #include <dynamixel_hardware_interface/dynamixel_const.h>
00018 #include <dynamixel_hardware_interface/dynamixel_io.h>
00019 #include <dynamixel_hardware_interface/serial_proxy.h>
00020 #include <dynamixel_hardware_interface/MotorState.h>
00021 #include <dynamixel_hardware_interface/MotorStateList.h>
00022 
00023 #include <ros/ros.h>
00024 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00025 #include <diagnostic_updater/update_functions.h>
00026 #include <diagnostic_msgs/DiagnosticArray.h>
00027 
00028 namespace dynamixel_hardware_interface
00029 {
00030 
00031 SerialProxy::SerialProxy(std::string port_name,
00032                          std::string port_namespace,
00033                          std::string baud_rate,
00034                          int min_motor_id,
00035                          int max_motor_id,
00036                          double update_rate,
00037                          double diagnostics_rate,
00038                          int error_level_temp,
00039                          int warn_level_temp)
00040     :port_name_(port_name),
00041      port_namespace_(port_namespace),
00042      baud_rate_(baud_rate),
00043      min_motor_id_(min_motor_id),
00044      max_motor_id_(max_motor_id),
00045      update_rate_(update_rate),
00046      diagnostics_rate_(diagnostics_rate),
00047      error_level_temp_(error_level_temp),
00048      warn_level_temp_(warn_level_temp),
00049      freq_status_(diagnostic_updater::FrequencyStatusParam(&update_rate_, &update_rate_, 0.1, 25))
00050 {
00051     current_state_ = MotorStateListPtr(new MotorStateList);
00052 
00053     motor_states_pub_ = nh_.advertise<MotorStateList>("motor_states/" + port_namespace_, 1000);
00054     diagnostics_pub_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1000);
00055 }
00056 
00057 SerialProxy::~SerialProxy()
00058 {
00059     if (feedback_thread_)
00060     {
00061         {
00062             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00063             terminate_feedback_ = true;
00064         }
00065         feedback_thread_->join();
00066         delete feedback_thread_;
00067     }
00068 
00069     if (diagnostics_thread_)
00070     {
00071         {
00072             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00073             terminate_diagnostics_ = true;
00074         }
00075         diagnostics_thread_->join();
00076         delete diagnostics_thread_;
00077     }
00078 
00079     delete dxl_io_;
00080 }
00081 
00082 bool SerialProxy::connect()
00083 {
00084     try
00085     {
00086         ROS_DEBUG("Constructing serial_proxy with %s at %s baud", port_name_.c_str(), baud_rate_.c_str());
00087         dxl_io_ = new DynamixelIO(port_name_, baud_rate_);
00088         if (!findMotors()) { return false; }
00089     }
00090     catch (flexiport::PortException pex)
00091     {
00092         ROS_ERROR("%s", pex.what());
00093         return false;
00094     }
00095     
00096     if (update_rate_ > 0)
00097     {
00098         terminate_feedback_ = false;
00099         feedback_thread_ = new boost::thread(boost::bind(&SerialProxy::updateMotorStates, this));
00100     }
00101     else
00102     {
00103         feedback_thread_ = NULL;
00104     }
00105 
00106     if (diagnostics_rate_ > 0)
00107     {
00108         terminate_diagnostics_ = false;
00109         diagnostics_thread_ = new boost::thread(boost::bind(&SerialProxy::publishDiagnosticInformation, this));
00110     }
00111     else
00112     {
00113         diagnostics_thread_ = NULL;
00114     }
00115     
00116     return true;
00117 }
00118 
00119 DynamixelIO* SerialProxy::getSerialPort()
00120 {
00121     return dxl_io_;
00122 }
00123 
00124 void SerialProxy::fillMotorParameters(const DynamixelData* motor_data)
00125 {
00126     int motor_id = motor_data->id;
00127     int model_number = motor_data->model_number;
00128     
00129     float voltage;
00130     dxl_io_->getVoltage(motor_id, voltage);
00131 
00132     std::stringstream ss;
00133     ss << "dynamixel/" << port_namespace_ << "/" << motor_id << "/";
00134     std::string prefix = ss.str();
00135 
00136     nh_.setParam(prefix + "model_number", model_number);
00137     nh_.setParam(prefix + "model_name", getMotorModelName(model_number));
00138     nh_.setParam(prefix + "min_angle", motor_data->cw_angle_limit);
00139     nh_.setParam(prefix + "max_angle", motor_data->ccw_angle_limit);
00140 
00141     double torque_per_volt = getMotorModelParams(model_number, TORQUE_PER_VOLT);
00142     nh_.setParam(prefix + "torque_per_volt", torque_per_volt);
00143     nh_.setParam(prefix + "max_torque", torque_per_volt * voltage);
00144 
00145     double velocity_per_volt = getMotorModelParams(model_number, VELOCITY_PER_VOLT);
00146     nh_.setParam(prefix + "velocity_per_volt", velocity_per_volt);
00147     nh_.setParam(prefix + "max_velocity", velocity_per_volt * voltage);
00148     nh_.setParam(prefix + "radians_second_per_encoder_tick", velocity_per_volt * voltage / DXL_MAX_VELOCITY_ENCODER);
00149 
00150     int encoder_resolution = (int) getMotorModelParams(model_number, ENCODER_RESOLUTION);
00151     double range_degrees = getMotorModelParams(model_number, RANGE_DEGREES);
00152     double range_radians = range_degrees * M_PI / 180.0;
00153 
00154     nh_.setParam(prefix + "encoder_resolution", encoder_resolution);
00155     nh_.setParam(prefix + "range_degrees", range_degrees);
00156     nh_.setParam(prefix + "range_radians", range_radians);
00157     nh_.setParam(prefix + "encoder_ticks_per_degree", encoder_resolution / range_degrees);
00158     nh_.setParam(prefix + "encoder_ticks_per_radian", encoder_resolution / range_radians);
00159     nh_.setParam(prefix + "degrees_per_encoder_tick", range_degrees / encoder_resolution);
00160     nh_.setParam(prefix + "radians_per_encoder_tick", range_radians / encoder_resolution);
00161 }
00162 
00163 bool SerialProxy::findMotors()
00164 {
00165     ROS_INFO("%s: Pinging motor IDs %d through %d...", port_namespace_.c_str(), min_motor_id_, max_motor_id_);
00166 
00167     XmlRpc::XmlRpcValue val;
00168     std::map<int, int> counts;
00169 
00170     for (int motor_id = min_motor_id_; motor_id <= max_motor_id_; ++motor_id)
00171     {
00172         if (dxl_io_->ping(motor_id))
00173         {
00174             const DynamixelData* motor_data;
00175             
00176             if ((motor_data = dxl_io_->getCachedParameters(motor_id)) == NULL)
00177             {
00178                 ROS_ERROR("Unable to retrieve cached paramaters for motor %d on port %s after successfull ping", motor_id, port_namespace_.c_str());
00179                 continue;
00180             }
00181             
00182             counts[motor_data->model_number] += 1;
00183             motor_static_info_[motor_id] = motor_data;
00184             fillMotorParameters(motor_data);
00185 
00186             motors_.push_back(motor_id);
00187             val[motors_.size()-1] = motor_id;
00188         }
00189     }
00190 
00191     if (motors_.empty())
00192     {
00193         ROS_WARN("%s: No motors found.", port_namespace_.c_str());
00194         return false;
00195     }
00196 
00197     nh_.setParam("dynamixel/" + port_namespace_ + "/connected_ids", val);
00198 
00199     std::stringstream ss;
00200     ss << port_namespace_ << ": Found " << motors_.size() << " motors - ";
00201 
00202     std::map<int, int>::iterator it;
00203     for (it = counts.begin(); it != counts.end(); ++it)
00204     {
00205         ss << it->second << " " << getMotorModelName(it->first) << " [";
00206         int c = 0;
00207         
00208         for (size_t i = 0; i < motors_.size(); ++i)
00209         {
00210             if (motor_static_info_[motors_[i]]->model_number == it->first)
00211             {
00212                 ss << motors_[i] << (++c == it->second ? "" : ", ");
00213             }
00214         }
00215         
00216         ss << "], ";
00217     }
00218 
00219     std::string status = ss.str();
00220     status.erase(status.size()-2);
00221     ROS_INFO("%s, initialization complete.", status.c_str());
00222     
00223     return true;
00224 }
00225 
00226 void SerialProxy::updateMotorStates()
00227 {    
00228     //ros::Rate rate(update_rate_);
00229     current_state_->motor_states.resize(motors_.size());
00230     dynamixel_hardware_interface::DynamixelStatus status;
00231     
00232     double allowed_time_usec = 1.0e6 / update_rate_;
00233     int sleep_time_usec = 0;
00234     
00235     struct timespec ts_now;
00236     double start_time_usec;
00237     double end_time_usec;
00238     
00239     while (nh_.ok())
00240     {
00241         {
00242             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00243             if (terminate_feedback_) { break; }
00244         }
00245 
00246         clock_gettime(CLOCK_REALTIME, &ts_now);
00247         start_time_usec = ts_now.tv_sec * 1.0e6 + ts_now.tv_nsec / 1.0e3;
00248         
00249         for (size_t i = 0; i < motors_.size(); ++i)
00250         {
00251             int motor_id = motors_[i];
00252 
00253             if (dxl_io_->getFeedback(motor_id, status))
00254             {
00255                 const DynamixelData* data = motor_static_info_[motor_id];
00256                 MotorState ms;
00257                 ms.timestamp = status.timestamp;
00258                 ms.id = motor_id;
00259                 ms.target_position = data->target_position;
00260                 ms.target_velocity = data->target_velocity;
00261                 ms.position = status.position;
00262                 ms.velocity = status.velocity;
00263                 ms.torque_limit = status.torque_limit;
00264                 ms.load = status.load;
00265                 ms.moving = status.moving;
00266                 ms.voltage = status.voltage;
00267                 ms.temperature = status.temperature;
00268                 current_state_->motor_states[i] = ms;
00269             }
00270             else
00271             {
00272                 ROS_DEBUG("Bad feedback received from motor %d on port %s", motor_id, port_namespace_.c_str());
00273             }
00274         }
00275 
00276         motor_states_pub_.publish(current_state_);
00277         freq_status_.tick();
00278 
00279         clock_gettime(CLOCK_REALTIME, &ts_now);
00280         end_time_usec = ts_now.tv_sec * 1.0e6 + ts_now.tv_nsec / 1.0e3;
00281         
00282         sleep_time_usec = allowed_time_usec - (end_time_usec - start_time_usec);
00283         
00284         // do millisecond resolution sleep
00285         if (sleep_time_usec >= 1000)
00286         {
00287             sleep_time_usec = (sleep_time_usec / 1000) * 1000;
00288             usleep(sleep_time_usec);
00289         }
00290         
00291         // ros sleep somehow takes a lot more cpu time
00292         //rate.sleep();
00293     }
00294 }
00295 
00296 void SerialProxy::publishDiagnosticInformation()
00297 {
00298     diagnostic_msgs::DiagnosticArray diag_msg;
00299     diagnostic_updater::DiagnosticStatusWrapper bus_status;
00300     ros::Rate rate(diagnostics_rate_);
00301 
00302     while (nh_.ok())
00303     {
00304         {
00305             boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
00306             if (terminate_diagnostics_) { break; }
00307         }
00308         
00309         double error_rate = dxl_io_->read_error_count / (double) dxl_io_->read_count;
00310 
00311         bus_status.clear();
00312         bus_status.name = "Dynamixel Serial Bus (" + port_namespace_ + ")";
00313         bus_status.hardware_id = "Dynamixel Serial Bus on port " + port_name_;
00314         bus_status.add("Baud Rate", baud_rate_);
00315         bus_status.add("Min Motor ID", min_motor_id_);
00316         bus_status.add("Max Motor ID", max_motor_id_);
00317         bus_status.addf("Error Rate", "%0.5f", error_rate);
00318         bus_status.summary(bus_status.OK, "OK");
00319         
00320         freq_status_.run(bus_status);
00321         
00322         if (error_rate > 0.05)
00323         {
00324             bus_status.mergeSummary(bus_status.WARN, "Too many errors while reading/writing to/from Dynamixel bus");
00325         }
00326         
00327         diag_msg.status.clear();
00328         diag_msg.header.stamp = ros::Time::now();
00329         diag_msg.status.push_back(bus_status);
00330         
00331         for (size_t i = 0; i < current_state_->motor_states.size(); ++i)
00332         {
00333             MotorState motor_state = current_state_->motor_states[i];
00334             int motor_id = motor_state.id;
00335             
00336             // check if current motor state was already populated by updateMotorStates thread
00337             if (motor_state.timestamp == 0.0) { continue; }
00338             
00339             const DynamixelData* data = motor_static_info_[motor_id];
00340             std::string mid_str = boost::lexical_cast<std::string>(motor_id);
00341             
00342             diagnostic_updater::DiagnosticStatusWrapper motor_status;
00343             
00344             motor_status.name = "Robotis Dynamixel Motor " + mid_str + " on port " + port_namespace_;
00345             motor_status.hardware_id = "ID " + mid_str + " on port " + port_name_;
00346             motor_status.add("Model Name", getMotorModelName(data->model_number).c_str());
00347             motor_status.addf("Firmware Version", "%d", data->firmware_version);
00348             motor_status.addf("Return Delay Time", "%d", data->return_delay_time);
00349             motor_status.addf("Minimum Voltage", "%0.1f", data->voltage_limit_low / 10.0);
00350             motor_status.addf("Maximum Voltage", "%0.1f", data->voltage_limit_high / 10.0);
00351             motor_status.addf("Maximum Torque", "%d", data->max_torque);
00352             motor_status.addf("Minimum Position (CW)", "%d", data->cw_angle_limit);
00353             motor_status.addf("Maximum Position (CCW)", "%d", data->ccw_angle_limit);
00354             motor_status.addf("Compliance Margin (CW)", "%d", data->cw_compliance_margin);
00355             motor_status.addf("Compliance Margin (CCW)", "%d", data->ccw_compliance_margin);
00356             motor_status.addf("Compliance Slope (CW)", "%d", data->cw_compliance_slope);
00357             motor_status.addf("Compliance Slope (CCW)", "%d", data->ccw_compliance_slope);
00358             motor_status.add("Torque Enabled", data->torque_enabled ? "True" : "False");
00359             
00360             motor_status.add("Moving", motor_state.moving ? "True" : "False");
00361             motor_status.addf("Target Position", "%d", motor_state.target_position);
00362             motor_status.addf("Target Velocity", "%d", motor_state.target_velocity);
00363             motor_status.addf("Position", "%d", motor_state.position);
00364             motor_status.addf("Velocity", "%d", motor_state.velocity);
00365             motor_status.addf("Position Error", "%d", motor_state.position - motor_state.target_position);
00366             motor_status.addf("Velocity Error", "%d", motor_state.velocity != 0 ? motor_state.velocity - motor_state.target_velocity : 0);
00367             motor_status.addf("Torque Limit", "%d", motor_state.torque_limit);
00368             motor_status.addf("Load", "%d", motor_state.load);
00369             motor_status.addf("Voltage", "%0.1f", motor_state.voltage / 10.0);
00370             motor_status.addf("Temperature", "%d", motor_state.temperature);
00371             
00372             motor_status.summary(motor_status.OK, "OK");
00373             
00374             if (motor_state.temperature >= error_level_temp_)
00375             {
00376                 motor_status.summary(motor_status.ERROR, "Overheating");
00377             }
00378             else if (motor_state.temperature >= warn_level_temp_)
00379             {
00380                 motor_status.summary(motor_status.WARN, "Very hot");
00381             }
00382             
00383             // if there was an overload or overheating error
00384             if (data->shutdown_error_time > 0.0)
00385             {
00386                 ROS_ERROR_THROTTLE(1, "%s: %s", port_namespace_.c_str(), data->error.c_str());
00387                 motor_status.mergeSummary(motor_status.ERROR, "Overload/Overheating error");
00388                 
00389                 // if current motor temperature is under control and
00390                 // we just arbitrarily waited 5 seconds just for kicks
00391                 if (motor_state.temperature < warn_level_temp_ &&
00392                     ros::Time::now().toSec() - data->shutdown_error_time >= 5.0)
00393                 {
00394                     dxl_io_->resetOverloadError(motor_id);
00395                     ROS_WARN("%s: Reset overload/overheating error on motor %d", port_namespace_.c_str(), motor_id);
00396                 }
00397             }
00398             else if (motor_state.torque_limit == 0)
00399             {
00400                 motor_status.mergeSummary(motor_status.ERROR, "Torque limit is 0");
00401             }
00402             
00403             diag_msg.status.push_back(motor_status);
00404         }
00405               
00406         diagnostics_pub_.publish(diag_msg);
00407         rate.sleep();
00408     }
00409 }
00410 
00411 }


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10