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
00036
00037 std::string searched_param;
00038 std::string port;
00039
00040
00041 _nh_private.searchParam("port", searched_param);
00042 _nh_private.param(searched_param, port, std::string());
00043
00044
00045 _nh_private.searchParam("joint_name", searched_param);
00046 _nh_private.param(searched_param, _joint_name, std::string());
00047
00048
00049 _nh_private.searchParam("joint_id", searched_param);
00050 _nh_private.param(searched_param, _joint_id, -1);
00051
00052
00053 _serial_device = "/dev/" + port;
00054
00055
00056
00057 int error = -1;
00058 std::string sem_file("/");
00059
00060 if (_joint_name == NECK) {
00061
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
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
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
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
00150 _driver->enableDriver();
00151 }
00152
00154
00155 void MotorControllerNode::spin()
00156 {
00157 while(_nh.ok()) {
00158 ros::spinOnce();
00159 _publish_rate.sleep();
00160
00161
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
00176
00177
00178 joint_factor = (_joint_name == NECK ? _pos_factor : TOTAL_ARMS_REDUCTION);
00179 factor = (2. * M_PI) / joint_factor;
00180
00181
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
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
00199 _data_msg.publish(msg_data);
00200
00201
00202
00203 double pos_factor, vel_factor;
00204 driverSensor_t dOdo;
00205
00206
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
00214 _driver->getDriverSensor(&dOdo);
00215
00216
00217 msg_odo.position = dOdo.p * pos_factor;
00218 msg_odo.velocity = dOdo.v * vel_factor;
00219 msg_odo.current = dOdo.i;
00220
00221
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
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
00307 _driver->setDriverHomePosition(req.home * _pos_factor / (2. * M_PI));
00308
00309
00310
00311 return true;
00312 }
00313
00315
00317
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
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