eyelids_node.cpp
Go to the documentation of this file.
00001 
00024 #include "eyelids_node.h"
00025 
00027 
00028 EyelidsNode::EyelidsNode(SerialCommunicationInterface *serial_comm) :
00029     _nh_private("~"),
00030     _publish_rate(10),
00031     _serial_comm(serial_comm)
00032 {
00033     // get ros params
00034     std::string searched_param;
00035     std::string port;
00036 
00037     // find the dev port name
00038     _nh_private.searchParam("port", searched_param);
00039     _nh_private.param(searched_param, port, std::string());
00040 
00041     // find the eye name
00042     _nh_private.searchParam("side", searched_param);
00043     _nh_private.param(searched_param, _side, std::string());
00044 
00045     // set full dev name
00046     _serial_comm->set_serial_device("/dev/" + port);
00047 
00048     _serial_comm->open_port(&_oldtio);
00049 
00050     // move eyelid to normal position
00051     if (_side == LEFT) {
00052         move(LEFT_EYELID_NORMAL);
00053     }
00054     else if (_side == RIGHT) {
00055         move(RIGHT_EYELID_NORMAL);
00056     }
00057 }
00058 
00060 
00061 EyelidsNode::~EyelidsNode()
00062 {
00063     // move eyelid to normal position
00064     if (_side == LEFT) {
00065         move(LEFT_EYELID_NORMAL);
00066     }
00067     else if (_side == RIGHT) {
00068         move(RIGHT_EYELID_NORMAL);
00069     }
00070 
00071     _serial_comm->close_port(&_oldtio);
00072 }
00073 
00075 
00076 void EyelidsNode::init()
00077 {
00078     _move_abs_pos_srv = _nh_private.advertiseService("move_abs_pos", &EyelidsNode::move_abs_pos, this);
00079     _move_str_pos_srv = _nh_private.advertiseService("move_str_pos", &EyelidsNode::move_str_pos, this);
00080 }
00081 
00083 
00084 void EyelidsNode::spin()
00085 {
00086     while(_nh.ok()) {
00087         ros::spinOnce();
00088         _publish_rate.sleep();
00089     }
00090 }
00091 
00093 
00094 bool EyelidsNode::move_abs_pos(motor_controller_msgs::MoveAbsPos::Request & req,
00095                                motor_controller_msgs::MoveAbsPos::Response & resp)
00096 {
00097     if (move(req.position) == -1) {
00098         return false;
00099     }
00100 
00101     return true;
00102 }
00103 
00105 
00106 bool EyelidsNode::move_str_pos(eyelids_msgs::MoveStrPos::Request & req, eyelids_msgs::MoveStrPos::Response & resp)
00107 {
00108     if (req.position == "open") {
00109         if (_side == LEFT) {
00110             move(LEFT_EYELID_MAX);
00111         }
00112         else if (_side == RIGHT) {
00113             move(RIGHT_EYELID_MAX);
00114         }
00115     }
00116     else if (req.position == "close") {
00117         if (_side == LEFT) {
00118             move(LEFT_EYELID_MIN);
00119         }
00120         else if (_side == RIGHT) {
00121             move(RIGHT_EYELID_MIN);
00122         }
00123     }
00124     else if (req.position == "normal") {
00125         if (_side == LEFT) {
00126             move(LEFT_EYELID_NORMAL);
00127         }
00128         else if (_side == RIGHT) {
00129             move(RIGHT_EYELID_NORMAL);
00130         }
00131     }
00132     else {
00133         ROS_ERROR("[EYELIDS] Error: wrong string position (%s), not available", std::string(req.position).c_str());
00134         return false;
00135     }
00136 
00137     return true;
00138 }
00139 
00141 
00142 int EyelidsNode::move(int position)
00143 {
00144     unsigned char bauds = BAUDS, motor_id, movement;
00145     int error = 0;
00146 
00147     if (_side == LEFT) {
00148         motor_id = SERVO_LEFT;
00149     }
00150     else if (_side == RIGHT) {
00151         motor_id = SERVO_RIGHT;
00152     }
00153 
00154     // it must be sended like a char, not an integer
00155     movement = (unsigned char) position;
00156 
00157     if (_serial_comm->send_character(&bauds) != 0) {
00158         ROS_ERROR("[EYELIDS] Error: sending bauds character");
00159         error = -1;
00160     }
00161     if (_serial_comm->send_character(&motor_id) != 0) {
00162         ROS_ERROR("[EYELIDS] Error: sending motor_id character");
00163         error = -1;
00164     }
00165     if (_serial_comm->send_character(&movement) != 0) {
00166         ROS_ERROR("[EYELIDS] Error: sending movement character");
00167         error = -1;
00168     }
00169     ROS_DEBUG("[EYELIDS] Command sent");
00170 
00171     return error;
00172 }
00173 


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