motor_controller_node.cpp
Go to the documentation of this file.
00001 
00026 #include "motor_controller_node.h"
00027 
00029 
00030 MotorControllerNode::MotorControllerNode(MotorDriverInterface *driver) :
00031     _nh_private("~"),
00032     _publish_rate(10),
00033     _driver(driver)
00034 {
00035     // get ros params
00036 
00037     std::string searched_param;
00038     std::string port;
00039 
00040     // find the dev port name
00041     _nh_private.searchParam("port", searched_param);
00042     _nh_private.param(searched_param, port, std::string());
00043 
00044     // find the joint name
00045     _nh_private.searchParam("joint_name", searched_param);
00046     _nh_private.param(searched_param, _joint_name, std::string());
00047 
00048     // find the joint id
00049     _nh_private.searchParam("joint_id", searched_param);
00050     _nh_private.param(searched_param, _joint_id, -1);
00051 
00052     // set full dev name
00053     _serial_device = "/dev/" + port;
00054 
00055     // set joint parameters
00056 
00057     int error = -1;
00058     std::string sem_file("/");
00059 
00060     if (_joint_name == NECK) {
00061         // neck
00062         switch(_joint_id) {
00063             case NECK_HOR:
00064                 sem_file += HOR_NECK_SEMAPHORE_FILE;
00065 
00066                 _pos_factor = TOTAL_NECK_HOR_REDUCTION;
00067                 _vel_factor = NECK_HOR_REDUCTION_FACTOR;
00068 
00069                 ROS_INFO("[MOTOR_CONTROLLER] Neck: Horizontal joint chosen. factor_position = %d\n", _pos_factor);
00070                 break;
00071 
00072             case NECK_VER:
00073                 sem_file += VER_NECK_SEMAPHORE_FILE;
00074 
00075                 _pos_factor = TOTAL_NECK_VER_REDUCTION;
00076                 _vel_factor = NECK_VER_REDUCTION_FACTOR;
00077 
00078                 ROS_INFO("[MOTOR_CONTROLLER] Neck: Vertical joint chosen. factor_position = %d\n", _pos_factor);
00079                 break;
00080 
00081             default:
00082                 ROS_WARN("[MOTOR_CONTROLLER] You must select the axis of the neck.\n");
00083                 exit(EXIT_FAILURE);
00084         }
00085     }
00086     else if (_joint_name == ARM) {
00087         // arms
00088         switch(_joint_id) {
00089             case ARM_LEFT:
00090                 sem_file += LEFT_ARM_SEMAPHORE_FILE;
00091 
00092                 ROS_INFO("[MOTOR_CONTROLLER] Arm: left joint chosen");
00093                 break;
00094 
00095             case ARM_RIGHT:
00096                 sem_file += RIGHT_ARM_SEMAPHORE_FILE;
00097 
00098                 ROS_INFO("[MOTOR_CONTROLLER] Arm: right joint chosen");
00099                 break;
00100 
00101             default:
00102                 ROS_WARN("[MOTOR_CONTROLLER] You must select the correct arm id.\n");
00103                 exit(EXIT_FAILURE);
00104         }
00105     }
00106     else {
00107         ROS_ERROR("[MOTOR_CONTROLLER] Error: no correct joint name param specified. Get: '%s', Expected: '%s' or '%s'", _joint_name.c_str(), NECK, ARM);
00108         exit(-1);
00109     }
00110 
00111     // trick for strings writable
00112     std::vector<char> wr_serial_device(_serial_device.begin(), _serial_device.end());
00113     wr_serial_device.push_back('\0');
00114     std::vector<char> wr_sem_file(sem_file.begin(), sem_file.end());
00115     wr_sem_file.push_back('\0');
00116 
00117     // init the communication
00118     if ((error = _driver->init(BAUDRATE, &wr_serial_device[0], &wr_sem_file[0])) != 0) {
00119         exit(error);
00120     }
00121 }
00122 
00124 
00125 MotorControllerNode::~MotorControllerNode()
00126 {
00127     _driver->disableDriver();
00128 }
00129 
00131 
00132 void MotorControllerNode::init()
00133 {
00134     _data_msg = _nh_private.advertise<motor_controller_msgs::Data>("data_pub", 100);
00135     _odo_msg = _nh_private.advertise<motor_controller_msgs::Odometry>("odo_pub", 100);
00136 
00137     _max_pos_srv = _nh_private.advertiseService("set_max_pos", &MotorControllerNode::set_max_pos, this);
00138     _min_pos_srv = _nh_private.advertiseService("set_min_pos", &MotorControllerNode::set_min_pos, this);
00139     _max_vel_srv = _nh_private.advertiseService("set_max_vel", &MotorControllerNode::set_max_vel, this);
00140     _max_acc_srv = _nh_private.advertiseService("set_max_acc", &MotorControllerNode::set_max_acc, this);
00141     _max_dec_srv = _nh_private.advertiseService("set_max_dec", &MotorControllerNode::set_max_dec, this);
00142     _cur_lim_srv = _nh_private.advertiseService("set_cur_lim", &MotorControllerNode::set_cur_lim, this);
00143     _odo_srv = _nh_private.advertiseService("set_odo", &MotorControllerNode::set_odometry_calibration, this);
00144 
00145     _mov_abs_pos_srv = _nh_private.advertiseService("mov_abs_pos", &MotorControllerNode::move_abs_pos, this);
00146     _mov_rel_pos_srv = _nh_private.advertiseService("mov_rel_pos", &MotorControllerNode::move_rel_pos, this);
00147     _mov_vel_srv = _nh_private.advertiseService("mov_vel", &MotorControllerNode::move_vel, this);
00148 
00149     // driver enable
00150     _driver->enableDriver();
00151 }
00152 
00154 
00155 void MotorControllerNode::spin()
00156 {
00157     while(_nh.ok()) {
00158         ros::spinOnce();
00159         _publish_rate.sleep();
00160 
00161         // publish motors information
00162         publish();
00163     }
00164 }
00165 
00167 
00168 void MotorControllerNode::publish()
00169 {
00170     motor_controller_msgs::Data msg_data;
00171     motor_controller_msgs::Odometry msg_odo;
00172     double factor;
00173     double joint_factor;
00174 
00175     // publish general data
00176 
00177     // get the factor
00178     joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00179     factor = (2. * M_PI) / joint_factor;
00180 
00181     // fill the message with positions
00182     msg_data.max_pos = _driver->getDriverMaxPos() * factor;
00183     msg_data.min_pos = _driver->getDriverMinPos() * factor;
00184 
00185     if (_joint_name == NECK) {
00186         factor = (2. * M_PI) / _vel_factor / 60.;
00187     }
00188     else {
00189         factor = (2. * M_PI) * PULSES_PER_REV / ARMS_REDUCTION_FACTOR / 60. / 60.;
00190     }
00191 
00192     // fill the message with the rest of data
00193     msg_data.max_vel = _driver->getDriverMaxVel() * factor;
00194     msg_data.max_acc = _driver->getDriverMaxAcc() * factor;
00195     msg_data.max_dec = _driver->getDriverMaxDec() * factor;
00196     msg_data.max_cur_lim = _driver->getDriverCurLim();
00197 
00198     // publish general data
00199     _data_msg.publish(msg_data);
00200 
00201     // publish odometry data
00202 
00203     double pos_factor, vel_factor;
00204     driverSensor_t dOdo;
00205 
00206     // get factors
00207     joint_factor = (_joint_name == NECK ? _pos_factor : ARMS_REDUCTION_FACTOR);
00208     pos_factor = (2. * M_PI) / joint_factor;
00209 
00210     joint_factor = (_joint_name == NECK ? _vel_factor : (PULSES_PER_REV / ARMS_REDUCTION_FACTOR));
00211     vel_factor = (2. * M_PI) / joint_factor / 60.;
00212 
00213     // get odometry information from motor driver
00214     _driver->getDriverSensor(&dOdo);
00215 
00216     // fill the message
00217     msg_odo.position = dOdo.p * pos_factor;
00218     msg_odo.velocity = dOdo.v * vel_factor;
00219     msg_odo.current = dOdo.i;
00220 
00221     // publish general data
00222     _odo_msg.publish(msg_odo);
00223 }
00224 
00226 
00227 bool MotorControllerNode::set_max_pos(motor_controller_msgs::Configuration::Request & req,
00228                                       motor_controller_msgs::Configuration::Response & resp)
00229 {
00230     /* from rad of the DOF to pulses of the engine */
00231     double joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00232     double factor = joint_factor / (2. * M_PI);
00233 
00234     _driver->setDriverMaxPos(req.max_pos * factor);
00235 
00236     return true;
00237 }
00238 
00240 
00241 bool MotorControllerNode::set_min_pos(motor_controller_msgs::Configuration::Request & req,
00242                                       motor_controller_msgs::Configuration::Response & resp)
00243 {
00244     double joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00245     double factor = joint_factor / (2. * M_PI);
00246 
00247     _driver->setDriverMinPos(req.min_pos * factor);
00248 
00249     return true;
00250 }
00251 
00253 
00254 bool MotorControllerNode::set_max_vel(motor_controller_msgs::Configuration::Request & req,
00255                                       motor_controller_msgs::Configuration::Response & resp)
00256 {
00257     double joint_factor = (_joint_name == NECK ? _vel_factor : TOTAL_ARMS_REDUCTION);
00258     double factor = joint_factor * 60. / (2. * M_PI);
00259 
00260     _driver->setDriverMaxVel(req.max_vel * factor);
00261 
00262     return true;
00263 }
00264 
00266 
00267 bool MotorControllerNode::set_max_acc(motor_controller_msgs::Configuration::Request & req,
00268                                       motor_controller_msgs::Configuration::Response & resp)
00269 {
00270     double joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00271     double factor = joint_factor / (2. * M_PI);
00272 
00273     _driver->setDriverMaxAcc(req.max_acc * factor);
00274 
00275     return true;
00276 }
00277 
00279 
00280 bool MotorControllerNode::set_max_dec(motor_controller_msgs::Configuration::Request & req,
00281                                       motor_controller_msgs::Configuration::Response & resp)
00282 {
00283     double joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00284     double factor = joint_factor / (2. * M_PI);
00285 
00286     _driver->setDriverMaxDec(req.max_dec * factor);
00287 
00288     return true;
00289 }
00290 
00292 
00293 bool MotorControllerNode::set_cur_lim(motor_controller_msgs::Configuration::Request & req,
00294                                       motor_controller_msgs::Configuration::Response & resp)
00295 {
00296     _driver->setDriverCurLim(req.cur);
00297 
00298     return true;
00299 }
00300 
00302 
00303 bool MotorControllerNode::set_odometry_calibration(motor_controller_msgs::Configuration::Request & req,
00304                                                    motor_controller_msgs::Configuration::Response & resp)
00305 {
00306     // neck
00307     _driver->setDriverHomePosition(req.home * _pos_factor / (2. * M_PI));
00308     // arms
00309     //    return setDriverOdoCalibration(_RSd, _semID, home * TOTAL_ARMS_REDUCTION / (2. * M_PI));
00310 
00311     return true;
00312 }
00313 
00315 
00317 // moves
00319 
00320 bool MotorControllerNode::move_abs_pos(motor_controller_msgs::MoveAbsPos::Request & req,
00321                                        motor_controller_msgs::MoveAbsPos::Response & resp)
00322 {
00323     ROS_DEBUG("[MOTOR_CONTROLLER] moveAbsPos pos = %d\n", req.position);
00324 
00325     long int joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00326     long int factor = joint_factor / (2. * M_PI);
00327 
00328     _driver->moveDriverAbsPos(req.position * factor);
00329 
00330     return true;
00331 }
00332 
00334 
00335 bool MotorControllerNode::move_rel_pos(motor_controller_msgs::MoveAbsPos::Request & req,
00336                                        motor_controller_msgs::MoveAbsPos::Response & resp)
00337 {
00338     ROS_DEBUG("[MOTOR_CONTROLLER] moveRelPos pos = %d\n", req.position);
00339 
00340     long int joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00341     long int factor = joint_factor / (2. * M_PI);
00342 
00343     _driver->moveDriverRelPos(req.position * factor);
00344 
00345     return true;
00346 }
00347 
00349 
00350 bool MotorControllerNode::move_vel(motor_controller_msgs::MoveAbsPos::Request & req,
00351                                    motor_controller_msgs::MoveAbsPos::Response & resp)
00352 {
00353     ROS_DEBUG("[MOTOR_CONTROLLER] moveVel vel = %d\n", req.position);
00354 
00355     // change velocity from rad/sec to rpm
00356     long int joint_factor = (_joint_name == NECK ? _vel_factor : TOTAL_ARMS_REDUCTION / PULSES_PER_REV);
00357     long int factor = joint_factor * 60. / (2. * M_PI);
00358 
00359     _driver->moveDriverVel(req.position * factor);
00360 
00361     return true;
00362 }
00363 


motor_controller
Author(s): Raul Perula-Martinez
autogenerated on Wed Apr 1 2015 10:17:09