Go to the documentation of this file.00001 #include "tibi_dabo_laser_3d_driver.h"
00002 #include "exceptions.h"
00003 #include "ros/ros.h"
00004
00005 TibiDaboLaser3dDriver::TibiDaboLaser3dDriver(void)
00006 {
00007
00008 this->servo=NULL;
00009 this->serial_number="";
00010 this->baudrate=1000000;
00011 this->servo_id=(unsigned char)-1;
00012 this->servo_config_file="";
00013 this->scan_enabled=false;
00014 this->fixed_angle=0.0;
00015 ros::param::get("~scan_velocity", this->new_velocity);
00016 ros::param::get("~min_angle", this->new_min_angle);
00017 ros::param::get("~max_angle", this->new_max_angle);
00018 ros::param::get("~scan_velocity", this->current_velocity);
00019 ros::param::get("~min_angle", this->current_min_angle);
00020 ros::param::get("~max_angle", this->current_max_angle);
00021 this->dir=right_left_scan;
00022 }
00023
00024 bool TibiDaboLaser3dDriver::openDriver(void)
00025 {
00026 double current_angle;
00027 std::string servo_name="laser_3d_servo";
00028
00029
00030 CDynamixelServer *dyn_server=CDynamixelServer::instance();
00031 try{
00032 if(dyn_server->get_num_buses()>0)
00033 {
00034 this->servo=new CDynamixelMotor(servo_name,
00035 this->serial_number,
00036 this->baudrate,
00037 this->servo_id);
00038 this->servo->enable();
00039 this->servo->load_config(this->servo_config_file);
00040 current_angle=this->servo->get_current_angle();
00041 if(this->scan_enabled)
00042 {
00043 this->servo->move_absolute_angle(this->current_min_angle,this->current_velocity);
00044 usleep(fabs((this->current_min_angle-current_angle)/this->current_velocity)*1000000);
00045 }
00046 {
00047 this->servo->move_absolute_angle(this->fixed_angle,this->current_velocity);
00048 usleep(fabs((this->fixed_angle-current_angle)/this->current_velocity)*1000000);
00049 }
00050 return true;
00051 }
00052 else
00053 return false;
00054 }catch(CException &e){
00055 if(this->servo!=NULL)
00056 {
00057 delete this->servo;
00058 this->servo=NULL;
00059 }
00060 ROS_WARN("%s",e.what().c_str());
00061 return false;
00062 }
00063
00064 return true;
00065 }
00066
00067 bool TibiDaboLaser3dDriver::closeDriver(void)
00068 {
00069 if(this->servo!=NULL)
00070 {
00071 delete this->servo;
00072 this->servo=NULL;
00073 }
00074 return true;
00075 }
00076
00077 bool TibiDaboLaser3dDriver::startDriver(void)
00078 {
00079 return true;
00080 }
00081
00082 bool TibiDaboLaser3dDriver::stopDriver(void)
00083 {
00084 return true;
00085 }
00086
00087 void TibiDaboLaser3dDriver::config_update(Config& new_cfg, uint32_t level)
00088 {
00089 this->lock();
00090
00091
00092
00093 switch(this->getState())
00094 {
00095 case TibiDaboLaser3dDriver::CLOSED:
00096 this->serial_number=new_cfg.bus_serial;
00097 this->baudrate=new_cfg.bus_baudrate;
00098 this->servo_id=new_cfg.servo_id;
00099 this->servo_config_file=new_cfg.config_file;
00100 this->scan_enabled=new_cfg.scan;
00101 this->fixed_angle=new_cfg.fixed_angle;
00102 break;
00103
00104 case TibiDaboLaser3dDriver::OPENED:
00105 this->new_velocity=new_cfg.scan_velocity;
00106 this->new_min_angle=new_cfg.min_angle;
00107 this->new_max_angle=new_cfg.max_angle;
00108 this->scan_enabled=new_cfg.scan;
00109 this->fixed_angle=new_cfg.fixed_angle;
00110 break;
00111
00112 case TibiDaboLaser3dDriver::RUNNING:
00113 break;
00114 }
00115
00116
00117 this->config_=new_cfg;
00118
00119 this->unlock();
00120 }
00121
00122 void TibiDaboLaser3dDriver::start_scan(void)
00123 {
00124 this->current_velocity=this->new_velocity;
00125 this->current_min_angle=this->new_min_angle;
00126 this->current_max_angle=this->new_max_angle;
00127 try{
00128 if(this->dir==left_right_scan)
00129 this->servo->move_absolute_angle(this->current_max_angle,this->current_velocity);
00130 else
00131 this->servo->move_absolute_angle(this->current_min_angle,this->current_velocity);
00132 }catch(CException &e){
00133 }
00134 }
00135
00136 bool TibiDaboLaser3dDriver::is_scan_done(void)
00137 {
00138 static ros::Time start_time = ros::Time::now();
00139 ros::Time current_time=ros::Time::now();
00140 ros::Duration scan_duration;
00141 double current_angle;
00142
00143 try{
00144 scan_duration=ros::Duration((this->current_max_angle-this->current_min_angle)/this->current_velocity);
00145 if(this->dir==left_right_scan)
00146 {
00147 current_angle=this->servo->get_current_angle();
00148 if(fabs((this->current_max_angle-current_angle)<1.0) || ((current_time-start_time)>scan_duration))
00149 {
00150 this->dir=right_left_scan;
00151 start_time=ros::Time::now();
00152 return true;
00153 }
00154 else
00155 return false;
00156 }
00157 else
00158 {
00159 current_angle=this->servo->get_current_angle();
00160 if((fabs(this->current_min_angle-current_angle)<1.0) || ((current_time-start_time)>scan_duration))
00161 {
00162 this->dir=left_right_scan;
00163 start_time=ros::Time::now();
00164 return true;
00165 }
00166 else
00167 return false;
00168 }
00169 }catch(CException &e){
00170 return false;
00171 }
00172 }
00173
00174 double TibiDaboLaser3dDriver::get_current_angle(void)
00175 {
00176 double angle=0.0;
00177
00178 try{
00179 angle=this->servo->get_current_angle();
00180 }catch(CException &e){
00181
00182 }
00183
00184 return angle;
00185 }
00186
00187 double TibiDaboLaser3dDriver::get_current_velocity(void)
00188 {
00189 double speed=0.0;
00190
00191 try{
00192 speed=this->servo->get_current_speed();
00193 }catch(CException &e){
00194 }
00195
00196 return speed;
00197 }
00198
00199 bool TibiDaboLaser3dDriver::is_scan_enabled(void)
00200 {
00201 return this->scan_enabled;
00202 }
00203
00204 double TibiDaboLaser3dDriver::get_fixed_angle(void)
00205 {
00206 return this->fixed_angle;
00207 }
00208
00209 void TibiDaboLaser3dDriver::move_absolute_angle(double angle)
00210 {
00211 try{
00212 this->servo->move_absolute_angle(angle,100);
00213 }catch(CException &e){
00214 }
00215 }
00216
00217 TibiDaboLaser3dDriver::~TibiDaboLaser3dDriver(void)
00218 {
00219 if(this->servo!=NULL)
00220 {
00221 delete this->servo;
00222 this->servo=NULL;
00223 }
00224 }