00001 #include <ros/ros.h>
00002 #include <boost/thread.hpp>
00003 #include <actionlib/server/action_server.h>
00004 #include <actionlib_msgs/GoalStatus.h>
00005 #include <dynamic_reconfigure/server.h>
00006 #include <unistd.h>
00007 #include <fcntl.h>
00008 #include <string.h>
00009 #include <queue>
00010 #include <algorithm>
00011 #include <wpa_supplicant_node/ScanAction.h>
00012 #include <wpa_supplicant_node/AssociateAction.h>
00013 #include <wpa_supplicant_node/AddNetwork.h>
00014 #include <wpa_supplicant_node/RemoveNetwork.h>
00015 #include <wpa_supplicant_node/SetNetworkState.h>
00016 #include <wpa_supplicant_node/SetNetworkParameters.h>
00017 #include <wpa_supplicant_node/NetworkList.h>
00018 #include <wpa_supplicant_node/FrequencyList.h>
00019 #include <wpa_supplicant_node/SecurityProperties.h>
00020 #include <wpa_supplicant_node/WpaSupplicantNodeConfig.h>
00021
00022 extern "C" {
00023 #include <includes.h>
00024 #include <common.h>
00025 #include <wpa_supplicant_i.h>
00026 #include <driver_i.h>
00027 #include <eloop.h>
00028 #include <drivers/driver.h>
00029 #include <scan.h>
00030 #include <bss.h>
00031 #include <config.h>
00032 #include <common/wpa_common.h>
00033 #include <common/ieee802_11_defs.h>
00034 #include "wpa_supplicant_node.h"
00035 #include <rsn_supp/wpa.h>
00036 }
00037
00038 typedef boost::function<void ()> WorkFunction;
00039
00040 void reconfigure(wpa_supplicant_node::WpaSupplicantNodeConfig &config, unsigned int& level);
00041
00042 static class RosApi {
00043
00044 bool initialized_;
00045 std::queue<WorkFunction> work_queue_;
00046 boost::condition_variable main_thread_cv_;
00047 enum main_thread_states { MAIN_THREAD_NOT_WAITING, MAIN_THREAD_WAITING, MAIN_THREAD_PAUSED };
00048 volatile main_thread_states main_thread_state_;
00049 int pipefd[2];
00050 boost::shared_ptr<boost::thread> ros_spin_loop_;
00051 volatile bool shutting_down_;
00052
00053 private:
00054 bool first_call_to_reconfigure;
00055
00056 public:
00057 boost::mutex mutex_;
00058
00059 RosApi()
00060 {
00061 shutting_down_ = false;
00062 main_thread_state_ = MAIN_THREAD_NOT_WAITING;
00063 }
00064
00065 void doWork()
00066 {
00067 char buffer[16];
00068
00069 ROS_INFO("doWork()");
00070
00071 {
00072 boost::mutex::scoped_lock lock(mutex_);
00073 mainThreadWait(lock);
00074 }
00075
00076 int bytes;
00077 do {
00078 bytes = read(pipefd[0], buffer, sizeof(buffer));
00079 ROS_INFO("doWork read %i bytes from the dummy fifo.", bytes);
00080 } while (bytes == sizeof(buffer));
00081
00082 while (1)
00083 {
00084 WorkFunction work;
00085 {
00086 boost::mutex::scoped_lock lock(mutex_);
00087 if (work_queue_.empty())
00088 break;
00089 work = work_queue_.front();
00090 work_queue_.pop();
00091 }
00092 work();
00093 }
00094 }
00095
00096 void addWork(const WorkFunction &f)
00097 {
00098 ROS_INFO("addWork()");
00099
00100 boost::mutex::scoped_lock lock(mutex_);
00101
00102 work_queue_.push(f);
00103 triggerWork();
00104 }
00105
00106 void triggerWork()
00107 {
00108 if (write(pipefd[1], pipefd, 1) != 1)
00109 ROS_ERROR("addWork Failed to write to wakeup pipe.");
00110 }
00111
00112 int init(int *argc, char ***argv)
00113 {
00114 ROS_INFO("ros_init");
00115
00116
00117 if (pipe2(pipefd, O_NONBLOCK | O_CLOEXEC))
00118 {
00119 ROS_FATAL("pipe2 failed: %s (%i)", strerror(errno), errno);
00120 return -1;
00121 }
00122 ROS_INFO("pipefds: %i <- %i", pipefd[0], pipefd[1]);
00123
00124 ros::init(*argc, *argv, "wpa_supplicant", ros::init_options::NoSigintHandler);
00125
00126 static dynamic_reconfigure::Server<wpa_supplicant_node::WpaSupplicantNodeConfig> srv;
00127 static dynamic_reconfigure::Server<wpa_supplicant_node::WpaSupplicantNodeConfig>::CallbackType f;
00128 f = boost::bind(&RosApi::reconfigure, this, _1, _2);
00129
00130 first_call_to_reconfigure = true;
00131 srv.setCallback(f);
00132 first_call_to_reconfigure = false;
00133
00134 ros_spin_loop_.reset(new boost::thread(boost::bind(&ros::spin)));
00135
00136 return 0;
00137 }
00138
00139 private:
00140 void reconfigure(wpa_supplicant_node::WpaSupplicantNodeConfig &config, unsigned int& level);
00141
00142 void waitForMainThreadState(boost::mutex::scoped_lock &lock, main_thread_states target)
00143 {
00144 while (main_thread_state_ != target && !shutting_down_)
00145 {
00146 main_thread_cv_.wait(lock);
00147 }
00148 }
00149
00150 void mainThreadWait(boost::mutex::scoped_lock &lock)
00151 {
00152 if (main_thread_state_ == MAIN_THREAD_WAITING)
00153 {
00154 main_thread_state_ = MAIN_THREAD_PAUSED;
00155 main_thread_cv_.notify_all();
00156 waitForMainThreadState(lock, MAIN_THREAD_NOT_WAITING);
00157 }
00158 }
00159
00160 public:
00161 bool waitForMainThread(boost::mutex::scoped_lock &lock)
00162 {
00163 main_thread_state_ = MAIN_THREAD_WAITING;
00164 triggerWork();
00165 waitForMainThreadState(lock, MAIN_THREAD_PAUSED);
00166 main_thread_state_ = MAIN_THREAD_NOT_WAITING;
00167 main_thread_cv_.notify_all();
00168 return !shutting_down_;
00169 }
00170
00171 void init2()
00172 {
00173 ROS_INFO("ros_init");
00174 eloop_register_read_sock(pipefd[0], &ros_do_work, NULL, NULL);
00175 }
00176
00177 void uninit()
00178 {
00179 ROS_INFO("ros_deinit");
00180
00181 boost::mutex::scoped_lock lock(mutex_);
00182 shutting_down_ = true;
00183 mainThreadWait(lock);
00184
00185 ros::shutdown();
00186
00187
00188 if (ros_spin_loop_ && !ros_spin_loop_->timed_join((boost::posix_time::milliseconds) 2000))
00189 ROS_ERROR("ROS thread did not die after two seconds. Exiting anyways. This is probably a bad sign.");
00190
00191 eloop_unregister_read_sock(pipefd[0]);
00192 close(pipefd[0]);
00193 close(pipefd[1]);
00194 }
00195 } g_ros_api;
00196
00197 typedef actionlib::ActionServer<wpa_supplicant_node::ScanAction> ScanActionServer;
00198 typedef actionlib::ActionServer<wpa_supplicant_node::AssociateAction> AssociateActionServer;
00199 const ScanActionServer ::GoalHandle null_scan_goal_handle_;
00200 const AssociateActionServer::GoalHandle null_associate_goal_handle_;
00201
00202 struct ros_interface
00203 {
00204 static char country_code[2];
00205 static struct wpa_global *global;
00206
00207 private:
00208 wpa_supplicant *wpa_s_;
00209
00210 ros::NodeHandle nh_;
00211
00212
00213 boost::recursive_mutex scan_mutex_;
00214 ScanActionServer sas_;
00215 std::queue<ScanActionServer::GoalHandle> scan_queue_;
00216 ScanActionServer::GoalHandle current_scan_;
00217 std::vector<int> current_scan_frequencies_;
00218
00219
00220 boost::recursive_mutex associate_mutex_;
00221 AssociateActionServer aas_;
00222 AssociateActionServer::GoalHandle active_association_;
00223 bool associate_work_requested_;
00224 std::queue<AssociateActionServer::GoalHandle> associate_goal_queue_;
00225 std::queue<AssociateActionServer::GoalHandle> associate_cancel_queue_;
00226
00227
00228 ros::ServiceServer add_network_service_;
00229 ros::ServiceServer remove_network_service_;
00230 ros::ServiceServer set_network_state_service_;
00231 ros::ServiceServer set_network_parameter_service_;
00232
00233
00234 ros::Publisher frequency_list_publisher_;
00235 ros::Publisher network_list_publisher_;
00236 ros::Publisher association_publisher_;
00237 ros::Publisher scan_publisher_;
00238
00239 public:
00240 ros_interface(const ros::NodeHandle &nh, wpa_supplicant *wpa_s) :
00241 wpa_s_(wpa_s),
00242 nh_(ros::NodeHandle(nh, wpa_s->ifname)),
00243 sas_(nh_, "scan", boost::bind(&ros_interface::scanGoalCallback, this, _1), boost::bind(&ros_interface::scanCancelCallback, this, _1), true),
00244 aas_(nh_, "associate", boost::bind(&ros_interface::associateGoalCallback, this, _1), boost::bind(&ros_interface::associateCancelCallback, this, _1), true)
00245 {
00246 add_network_service_ = nh_.advertiseService("add_network", &ros_interface::addNetwork, this);
00247 remove_network_service_ = nh_.advertiseService("remove_network", &ros_interface::removeNetwork, this);
00248 set_network_state_service_ = nh_.advertiseService("set_network_state", &ros_interface::setNetworkState, this);
00249 set_network_parameter_service_ = nh_.advertiseService("set_network_parameter", &ros_interface::setNetworkParameter, this);
00250
00251 network_list_publisher_ = nh_.advertise<wpa_supplicant_node::NetworkList>("network_list", 1, true);
00252 frequency_list_publisher_ = nh_.advertise<wpa_supplicant_node::FrequencyList>("frequency_list", 1, true);
00253 association_publisher_ = nh_.advertise<wpa_supplicant_node::AssociateFeedback>("association_state", 1, true);
00254 scan_publisher_ = nh_.advertise<wpa_supplicant_node::ScanResult>("scan_results", 1, true);
00255
00256 if (!ros_interface::global)
00257 {
00258 ros_interface::global = wpa_s->global;
00259 ROS_INFO("calling setDriverCountry");
00260 setDriverCountry();
00261 }
00262
00263 eloop_register_timeout(10, 0, delayedPublishFrequencyList, wpa_s, wpa_s);
00264 publishNetworkList();
00265 publishUnassociated();
00266 }
00267
00268 static void setCountryCode(const char *country_code)
00269 {
00270 if (strlen(country_code) != 2)
00271 {
00272 ROS_ERROR("Invalid country code %s", country_code);
00273 return;
00274 }
00275
00276 ros_interface::country_code[0] = country_code[0];
00277 ros_interface::country_code[1] = country_code[1];
00278
00279 if (global && global->ifaces && global->ifaces->ros_api)
00280 {
00281 global->ifaces->ros_api->setDriverCountry();
00282 eloop_register_timeout(0, 100000, delayedPublishFrequencyList, global->ifaces, NULL);
00283 }
00284 }
00285
00286 void publishUnassociated()
00287 {
00288 wpa_supplicant_node::AssociateFeedback fbk;
00289 association_publisher_.publish(fbk);
00290 }
00291
00292 void ifaceIdle()
00293 {
00294 }
00295
00296 void scanCompleted(wpa_scan_results *scan_res)
00297 {
00298 ROS_INFO("scanCompleted");
00299
00300 boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00301
00302 eloop_cancel_timeout(scanTimeoutHandler, wpa_s_, NULL);
00303
00304 wpa_supplicant_node::ScanResult rslt;
00305 if (scan_res)
00306 {
00307 fillRosResp(rslt, *scan_res);
00308 scan_publisher_.publish(rslt);
00309 }
00310
00311 if (current_scan_ == null_scan_goal_handle_)
00312 ROS_ERROR("scanCompleted with current_scan_ not set.");
00313 else
00314 {
00315 if (scan_res)
00316 {
00317
00318 ROS_INFO("Scan completed successfully.");
00319 current_scan_.setSucceeded(rslt);
00320 }
00321 else
00322 {
00323 ROS_INFO("Scan failed.");
00324 current_scan_.setAborted(rslt);
00325 }
00326 current_scan_ = null_scan_goal_handle_;
00327 lockedScanTryActivate();
00328 }
00329 }
00330
00331 void assocFailed(const u8 *bssid, const char *s)
00332 {
00333 ROS_INFO("assocFailed");
00334 boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00335
00336 eloop_cancel_timeout(associateTimeoutHandler, wpa_s_, NULL);
00337
00338 if (active_association_ == null_associate_goal_handle_)
00339 {
00340 ROS_ERROR("Got disassociation with no active association on BSSID: " MACSTR " (%s)", MAC2STR(bssid), s);
00341 }
00342 else
00343 {
00344 if (os_memcmp(bssid, &active_association_.getGoal()->bssid, ETH_ALEN) &&
00345 !is_zero_ether_addr(bssid))
00346 {
00347 ROS_ERROR("Got disassociation on unexpected BSSID: " MACSTR " (%s)", MAC2STR(bssid), s);
00348 }
00349
00350 active_association_.setAborted();
00351 active_association_ = null_associate_goal_handle_;
00352 }
00353
00354 publishUnassociated();
00355 requestAssociateWork();
00356 }
00357
00358 void assocSucceeded()
00359 {
00360 ROS_INFO("assocSucceeded");
00361 boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00362
00363 eloop_cancel_timeout(associateTimeoutHandler, wpa_s_, NULL);
00364
00365 wpa_supplicant_node::AssociateFeedback fbk;
00366 fbk.associated = true;
00367 fillRosBss(fbk.bss, *wpa_s_->current_bss);
00368
00369
00370 active_association_.publishFeedback(fbk);
00371 association_publisher_.publish(fbk);
00372 }
00373
00374 void publishNetworkList()
00375 {
00376 wpa_supplicant_node::NetworkList netlist;
00377
00378 for (wpa_ssid *ssid = wpa_s_->conf->ssid; ssid; ssid = ssid->next)
00379 {
00380 wpa_supplicant_node::Network net;
00381 net.network_id = ssid->id;
00382 net.enabled = !ssid->disabled;
00383
00384 char **parameters = wpa_config_get_all(ssid, 0);
00385 for (char **cur_parameter = parameters; *cur_parameter; cur_parameter += 2)
00386 {
00387 wpa_supplicant_node::NetworkParameter param;
00388 param.key = cur_parameter[0];
00389 param.value = cur_parameter[1];
00390 net.parameters.push_back(param);
00391 free(cur_parameter[0]);
00392 free(cur_parameter[1]);
00393 }
00394 free(parameters);
00395
00396 netlist.networks.push_back(net);
00397 }
00398
00399 network_list_publisher_.publish(netlist);
00400 }
00401
00402 private:
00403 void setDriverCountry()
00404 {
00405 if (!ros_interface::country_code[0])
00406 return;
00407
00408 wpa_s_->conf->country[0] = ros_interface::country_code[0];
00409 wpa_s_->conf->country[1] = ros_interface::country_code[1];
00410
00411 if (wpa_drv_set_country(wpa_s_, wpa_s_->conf->country))
00412 {
00413 ROS_ERROR("Failed to set driver country code %s", ros_interface::country_code);
00414 return;
00415 }
00416
00417 struct wpa_supplicant *wpa_s_iter = wpa_s_->next;
00418
00419 while (wpa_s_iter)
00420 {
00421 if (wpa_s_iter->ros_api)
00422 eloop_register_timeout(0, 100000, delayedPublishFrequencyList, wpa_s_iter, NULL);
00423 wpa_s_iter = wpa_s_iter->next;
00424 }
00425 }
00426
00427 static void delayedPublishFrequencyList(void *wpa_s, void *repeat_p)
00428 {
00429 ((wpa_supplicant *) wpa_s)->ros_api->publishFrequencyList();
00430
00431 if (repeat_p != NULL)
00432 eloop_register_timeout(10, 0, delayedPublishFrequencyList, wpa_s, repeat_p);
00433 }
00434
00435 void publishFrequencyList()
00436 {
00437 wpa_supplicant_node::FrequencyList f;
00438
00439 uint16_t num_modes, flags;
00440 struct hostapd_hw_modes *modes;
00441
00442 modes = wpa_drv_get_hw_feature_data(wpa_s_, &num_modes, &flags);
00443
00444 if (modes != NULL)
00445 {
00446 for (struct hostapd_hw_modes *mode = modes; mode < modes + num_modes; mode++)
00447 {
00448 if (mode->channels)
00449 {
00450 for (struct hostapd_channel_data *channel = mode->channels;
00451 channel < mode->channels + mode->num_channels;
00452 channel++)
00453 {
00454 if (!(channel->flag & HOSTAPD_CHAN_DISABLED) &&
00455 std::find(f.frequencies.begin(), f.frequencies.end(), channel->freq) == f.frequencies.end())
00456 {
00457 f.frequencies.push_back(channel->freq);
00458 }
00459 }
00460 free(mode->channels);
00461 }
00462 if (mode->rates)
00463 free(mode->rates);
00464 }
00465 free(modes);
00466 }
00467 else
00468 {
00469 for (int i = 2412; i <= 2462; i += 5)
00470 f.frequencies.push_back(i);
00471 for (int i = 5180; i <= 5320; i += 20)
00472 f.frequencies.push_back(i);
00473 for (int i = 5500; i <= 5580; i += 20)
00474 f.frequencies.push_back(i);
00475 for (int i = 5500; i <= 5580; i += 20)
00476 f.frequencies.push_back(i);
00477 for (int i = 5680; i <= 5700; i += 20)
00478 f.frequencies.push_back(i);
00479 for (int i = 5745; i <= 5825; i += 20)
00480 f.frequencies.push_back(i);
00481 }
00482
00483 frequency_list_publisher_.publish(f);
00484 }
00485
00486 bool addNetwork(wpa_supplicant_node::AddNetwork::Request &req, wpa_supplicant_node::AddNetwork::Response &rsp)
00487 {
00488 ROS_INFO("addNetwork");
00489
00490 boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00491 if (!g_ros_api.waitForMainThread(lock))
00492 return false;
00493
00494
00495 return true;
00496 }
00497
00498 bool removeNetwork(wpa_supplicant_node::RemoveNetwork::Request &req, wpa_supplicant_node::RemoveNetwork::Response &rsp)
00499 {
00500 ROS_INFO("removeNetwork");
00501
00502 boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00503 if (!g_ros_api.waitForMainThread(lock))
00504 return false;
00505
00506
00507 return true;
00508 }
00509
00510 bool setNetworkState(wpa_supplicant_node::SetNetworkState::Request &req, wpa_supplicant_node::SetNetworkState::Response &rsp)
00511 {
00512 ROS_INFO("setNetworkState");
00513
00514 boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00515 if (!g_ros_api.waitForMainThread(lock))
00516 return false;
00517
00518
00519 return true;
00520 }
00521
00522 bool setNetworkParameter(wpa_supplicant_node::SetNetworkParameters::Request &req, wpa_supplicant_node::SetNetworkParameters::Response &rsp)
00523 {
00524 ROS_INFO("setNetworkParameter");
00525
00526 boost::mutex::scoped_lock lock(g_ros_api.mutex_);
00527 if (!g_ros_api.waitForMainThread(lock))
00528 return false;
00529
00530
00531 return true;
00532 }
00533 void requestAssociateWork()
00534 {
00535 associate_work_requested_ = true;
00536 g_ros_api.addWork(boost::bind(&ros_interface::associateWork, this));
00537 }
00538
00539 void stopActiveAssociation()
00540 {
00541 ROS_INFO("stopActiveAssociation()");
00542 if (active_association_ == null_associate_goal_handle_)
00543 ROS_ERROR("stopActiveAssociation called with no active association.");
00544
00545 wpa_supplicant_disassociate(wpa_s_, WLAN_REASON_DEAUTH_LEAVING);
00546 }
00547
00548 void startActiveAssociation(AssociateActionServer::GoalHandle &gh)
00549 {
00550 ROS_INFO("startActiveAssociation()");
00551 if (active_association_ != null_associate_goal_handle_)
00552 ROS_ERROR("startActiveAssociation called with no active association.");
00553
00554 wpa_bss *bss;
00555 boost::shared_ptr<const wpa_supplicant_node::AssociateGoal> goal = gh.getGoal();
00556 const u8 *bssid = &goal->bssid[0];
00557 std::string ssid = goal->ssid;
00558
00559 bss = wpa_bss_get(wpa_s_, bssid, (u8 *) ssid.c_str(), ssid.length());
00560
00561 wpa_ssid *net;
00562 for (net = wpa_s_->conf->ssid; net != NULL; net = net->next)
00563 if (ssid.length() == net->ssid_len && !os_memcmp(ssid.c_str(), net->ssid, net->ssid_len))
00564 break;
00565
00566 if (bss && net)
00567 {
00568 gh.setAccepted();
00569 active_association_ = gh;
00570 wpa_supplicant_node::AssociateFeedback fbk;
00571 fbk.associated = false;
00572 active_association_.publishFeedback(fbk);
00573
00574 eloop_register_timeout(5, 0, associateTimeoutHandler, wpa_s_, NULL);
00575
00576
00577
00578 wpa_supplicant_associate(wpa_s_, bss, net);
00579 }
00580 else
00581 {
00582 if (!bss)
00583 ROS_ERROR("startActiveAssociation could not find requested bss.");
00584 if (!net)
00585 ROS_ERROR("startActiveAssociation could not find requested ssid.");
00586 gh.setRejected();
00587 }
00588 ROS_INFO("startActiveAssociation() done");
00589 }
00590
00591 void associateWork()
00592 {
00593 boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00594 associate_work_requested_ = false;
00595
00596 while (!associate_cancel_queue_.empty())
00597 {
00598 AssociateActionServer::GoalHandle gh = associate_cancel_queue_.front();
00599 associate_cancel_queue_.pop();
00600 if (gh == active_association_)
00601 {
00602 stopActiveAssociation();
00603 return;
00604 }
00605 else
00606 gh.setCanceled();
00607 }
00608
00609 while (associate_goal_queue_.size() > 1)
00610 {
00611 AssociateActionServer::GoalHandle gh = associate_goal_queue_.front();
00612 associate_goal_queue_.pop();
00613 gh.setRejected();
00614 }
00615
00616 if (!associate_goal_queue_.empty())
00617 {
00618 AssociateActionServer::GoalHandle gh = associate_goal_queue_.front();
00619 if (active_association_ != null_associate_goal_handle_ && active_association_.getGoal()->ssid != gh.getGoal()->ssid)
00620 {
00621 ROS_INFO("Unassociating prior to ESSID switch.");
00622 stopActiveAssociation();
00626
00627
00628 }
00629 else
00630 {
00631 if (active_association_ != null_associate_goal_handle_)
00632 {
00633 active_association_.setAborted();
00634 active_association_ = null_associate_goal_handle_;
00635 }
00636
00637 associate_goal_queue_.pop();
00638
00639 startActiveAssociation(gh);
00640 }
00641 }
00642 }
00643
00644 void associateGoalCallback(AssociateActionServer::GoalHandle &gh)
00645 {
00646 boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00647
00648 associate_goal_queue_.push(gh);
00649 requestAssociateWork();
00650 }
00651
00652 void associateCancelCallback(AssociateActionServer::GoalHandle &gh)
00653 {
00654 boost::recursive_mutex::scoped_lock lock(associate_mutex_);
00655
00656 associate_cancel_queue_.push(gh);
00657 requestAssociateWork();
00658 }
00659
00660 void fillSecurityInformation(wpa_supplicant_node::SecurityProperties &props, struct wpa_ie_data &ie_data)
00661 {
00662
00663 if (ie_data.key_mgmt & WPA_KEY_MGMT_PSK)
00664 props.key_mgmt.push_back("wpa-psk");
00665 if (ie_data.key_mgmt & WPA_KEY_MGMT_FT_PSK)
00666 props.key_mgmt.push_back("wpa-ft-psk");
00667 if (ie_data.key_mgmt & WPA_KEY_MGMT_PSK_SHA256)
00668 props.key_mgmt.push_back("wpa-psk-sha256");
00669 if (ie_data.key_mgmt & WPA_KEY_MGMT_IEEE8021X)
00670 props.key_mgmt.push_back("wpa-eap");
00671 if (ie_data.key_mgmt & WPA_KEY_MGMT_FT_IEEE8021X)
00672 props.key_mgmt.push_back("wpa-ft-eap");
00673 if (ie_data.key_mgmt & WPA_KEY_MGMT_IEEE8021X_SHA256)
00674 props.key_mgmt.push_back("wpa-eap-sha256");
00675 if (ie_data.key_mgmt & WPA_KEY_MGMT_NONE)
00676 props.key_mgmt.push_back("wpa-none");
00677
00678
00679 switch (ie_data.group_cipher) {
00680 case WPA_CIPHER_WEP40:
00681 props.group.push_back("wep40");
00682 break;
00683 case WPA_CIPHER_TKIP:
00684 props.group.push_back("tkip");
00685 break;
00686 case WPA_CIPHER_CCMP:
00687 props.group.push_back("ccmp");
00688 break;
00689 case WPA_CIPHER_WEP104:
00690 props.group.push_back("wep104");
00691 break;
00692 }
00693
00694
00695 if (ie_data.pairwise_cipher & WPA_CIPHER_TKIP)
00696 props.pairwise.push_back("tkip");
00697 if (ie_data.pairwise_cipher & WPA_CIPHER_CCMP)
00698 props.pairwise.push_back("ccmp");
00699 }
00700
00701 void fillRosBss(wpa_supplicant_node::Bss &ros_bss, wpa_bss &bss)
00702 {
00703 ros_bss.stamp = ros::Time(bss.last_update.sec + 1e-6 * bss.last_update.usec);
00704 ros_bss.ssid.assign((const char *) bss.ssid, bss.ssid_len);
00705 memcpy(&ros_bss.bssid[0], bss.bssid, sizeof(bss.bssid));
00706 ros_bss.frequency = bss.freq;
00707 ros_bss.beacon_interval = bss.beacon_int;
00708 ros_bss.capabilities = bss.caps;
00709 ros_bss.quality = bss.qual;
00710 ros_bss.level = bss.level;
00711 ros_bss.noise = bss.noise;
00712
00713 struct wpa_ie_data wpa_data;
00714 const u8 *ie;
00715
00716 os_memset(&wpa_data, 0, sizeof(wpa_data));
00717 ie = wpa_bss_get_vendor_ie(&bss, WPA_IE_VENDOR_TYPE);
00718 if (ie)
00719 wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00720 fillSecurityInformation(ros_bss.wpa, wpa_data);
00721
00722 os_memset(&wpa_data, 0, sizeof(wpa_data));
00723 ie = wpa_bss_get_ie(&bss, WLAN_EID_RSN);
00724 if (ie)
00725 wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00726 fillSecurityInformation(ros_bss.rsn, wpa_data);
00727 }
00728
00729 void fillRosResp(wpa_supplicant_node::ScanResult &rslt, wpa_scan_results &scan_res)
00730 {
00731 rslt.success = true;
00732 rslt.bss.clear();
00733 for (size_t i = 0; i < scan_res.num; i++)
00734 {
00735 wpa_scan_res &cur = *scan_res.res[i];
00736 wpa_supplicant_node::Bss bss;
00737 const u8* ssid_ie = wpa_scan_get_ie(&cur, WLAN_EID_SSID);
00738 int ssid_len = ssid_ie ? ssid_ie[1] : 0;
00739 const char *ssid = ssid_ie ? (const char *) ssid_ie + 2 : "";
00740 bss.ssid.assign(ssid, ssid_len);
00741 memcpy(&bss.bssid[0], cur.bssid, sizeof(bss.bssid));
00742 bss.noise = cur.noise;
00743 bss.quality = cur.qual;
00744 bss.level = cur.level;
00745 bss.capabilities = cur.caps;
00746 bss.beacon_interval = cur.beacon_int;
00747 bss.frequency = cur.freq;
00748 bss.stamp = ros::Time::now() - ros::Duration(cur.age / 1000.0);
00749
00750 struct wpa_ie_data wpa_data;
00751 const u8 *ie;
00752
00753 os_memset(&wpa_data, 0, sizeof(wpa_data));
00754 ie = wpa_scan_get_vendor_ie(&cur, WPA_IE_VENDOR_TYPE);
00755 if (ie)
00756 wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00757 fillSecurityInformation(bss.wpa, wpa_data);
00758
00759 os_memset(&wpa_data, 0, sizeof(wpa_data));
00760 ie = wpa_scan_get_ie(&cur, WLAN_EID_RSN);
00761 if (ie)
00762 wpa_parse_wpa_ie(ie, 2 + ie[1], &wpa_data);
00763 fillSecurityInformation(bss.rsn, wpa_data);
00764 rslt.bss.push_back(bss);
00765 }
00766 }
00767
00768 void scanGoalCallback(ScanActionServer::GoalHandle &gh)
00769 {
00770 boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00771
00772 ROS_INFO("scanGoalCallback()");
00773
00774 scan_queue_.push(gh);
00775
00776 if (current_scan_ == null_scan_goal_handle_)
00777 g_ros_api.addWork(boost::bind(&ros_interface::scanTryActivate, this));
00778 }
00779
00780 void scanCancelCallback(ScanActionServer::GoalHandle &gh)
00781 {
00782 boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00783
00784 unsigned int status = gh.getGoalStatus().status;
00785
00786 if (current_scan_ == gh)
00787 {
00788 g_ros_api.addWork(boost::bind(&ros_interface::scanCancel, this, gh));
00789 }
00790 else
00791 {
00792 if (status != actionlib_msgs::GoalStatus::PREEMPTING)
00793 ROS_ERROR("scanCancelCallback called with unexpected goal status %i", status);
00794 gh.setCanceled();
00795 }
00796 }
00797
00798 void scanCancel(ScanActionServer::GoalHandle &gh)
00799 {
00800 ROS_INFO("scanCancel()");
00801 boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00802
00803
00804 if (current_scan_ == gh)
00805 {
00806
00807 gh.setCanceled();
00808 current_scan_ = null_scan_goal_handle_;
00809 scanTryActivate();
00810 }
00811 }
00812
00813 void lockedScanTryActivate()
00814 {
00815
00816 ROS_INFO("scanTryActivate()");
00817
00818
00819 while (!scan_queue_.empty() && current_scan_ == null_scan_goal_handle_)
00820 {
00821 current_scan_ = scan_queue_.front();
00822 scan_queue_.pop();
00823
00824
00825
00826 if (current_scan_.getGoalStatus().status != actionlib_msgs::GoalStatus::PENDING)
00827 {
00828 ROS_INFO("Skipping canceled scan.");
00829 current_scan_ = null_scan_goal_handle_;
00830 continue;
00831 }
00832
00833 boost::shared_ptr<const wpa_supplicant_node::ScanGoal> goal = current_scan_.getGoal();
00834 struct wpa_driver_scan_params wpa_req;
00835 bzero(&wpa_req, sizeof(wpa_req));
00836
00837 std::string err = fillWpaReq(goal, wpa_req);
00838
00839 if (err.empty())
00840 {
00841 current_scan_.setAccepted();
00842
00843 ROS_INFO("Starting scan.");
00844
00845
00846
00847 int timeout;
00848 if (wpa_req.freqs == 0)
00849 timeout = 10000;
00850 else
00851 timeout = goal->frequencies.size() * 550;
00852 eloop_register_timeout(timeout / 1000, 1000 * (timeout % 1000), scanTimeoutHandler, wpa_s_, NULL);
00853 wpa_supplicant_trigger_scan(wpa_s_, &wpa_req);
00854 }
00855 else
00856 {
00857 current_scan_.setRejected(wpa_supplicant_node::ScanResult(), err);
00858 current_scan_ = null_scan_goal_handle_;
00859 continue;
00860 }
00861 }
00862
00863 ROS_INFO("Leaving scanTryActivate");
00864 if (scan_queue_.empty())
00865 ROS_INFO("scan_queue_ is empty.");
00866 if (current_scan_ != null_scan_goal_handle_)
00867 ROS_INFO("A scan is active.");
00868 }
00869
00870 static void scanTimeoutHandler(void *wpa_s, void *unused)
00871 {
00872 ROS_INFO("Scan timeout!");
00873 ((wpa_supplicant *) wpa_s)->ros_api->scanCompleted(NULL);
00874 }
00875
00876 static void associateTimeoutHandler(void *wpa_s, void *unused)
00877 {
00878 ROS_INFO("Associate timeout!");
00879 static const u8 zeroMAC[] = "\0\0\0\0\0\0";
00880 ((wpa_supplicant *) wpa_s)->ros_api->assocFailed(zeroMAC, "Associatiot timed out.");
00881 }
00882
00883 std::string fillWpaReq(boost::shared_ptr<const wpa_supplicant_node::ScanGoal> &g, struct wpa_driver_scan_params &wpa_req)
00884 {
00885 wpa_req.num_ssids = g->ssids.size();
00886
00887 #define QUOTEME(x) #x
00888 if (wpa_req.num_ssids > WPAS_MAX_SCAN_SSIDS)
00889 return "Too many ESSIDs in scan request. Maximum number is "QUOTEME(WPAS_MAX_SCAN_SSIDS)".";
00890
00891 for (unsigned int i = 0; i < wpa_req.num_ssids; i++)
00892 {
00893 wpa_req.ssids[i].ssid = (const u8 *) g->ssids[i].c_str();
00894 wpa_req.ssids[i].ssid_len = g->ssids[i].size();
00895
00896 if (wpa_req.ssids[i].ssid_len > WPA_MAX_SSID_LEN)
00897 return "Ssid is too long. Maximum length is "QUOTEME(WPA_MAX_SSID_LEN)" characters.";
00898 }
00899 #undef QUOTEME
00900
00901 current_scan_frequencies_.clear();
00902 int num_frequencies = g->frequencies.size();
00903
00904 if (num_frequencies > 0)
00905 {
00906 for (int i = 0; i < num_frequencies; i++)
00907 current_scan_frequencies_.push_back(g->frequencies[i]);
00908 current_scan_frequencies_.push_back(0);
00909 wpa_req.freqs = ¤t_scan_frequencies_[0];
00910 }
00911
00912 return "";
00913 }
00914
00915 void scanTryActivate()
00916 {
00917 boost::recursive_mutex::scoped_lock lock(scan_mutex_);
00918 lockedScanTryActivate();
00919 }
00920 };
00921
00922 struct wpa_global *ros_interface::global = NULL;
00923 char ros_interface::country_code[2] = {0, 0};
00924
00925 void RosApi::reconfigure(wpa_supplicant_node::WpaSupplicantNodeConfig &config, unsigned int& level)
00926 {
00927 boost::mutex::scoped_lock lock(mutex_);
00928
00929 if (!first_call_to_reconfigure)
00930 {
00931 if (!waitForMainThread(lock))
00932 return;
00933 }
00934
00935 ros_interface::setCountryCode(config.country_code.c_str());
00936 }
00937
00938 extern "C" {
00939
00940 int ros_init(int *argc, char ***argv)
00941 {
00942 return g_ros_api.init(argc, argv);
00943 }
00944
00945 void ros_init2()
00946 {
00947 return g_ros_api.init2();
00948 }
00949
00950 void ros_deinit()
00951 {
00952 g_ros_api.uninit();
00953 }
00954
00955 void ros_add_iface(wpa_supplicant *wpa_s)
00956 {
00957 wpa_s->ros_api = new ros_interface(ros::NodeHandle("wifi"), wpa_s);
00958 }
00959
00960 void ros_remove_iface(wpa_supplicant *wpa_s)
00961 {
00962 delete wpa_s->ros_api;
00963 wpa_s->ros_api = NULL;
00964 }
00965
00966 void ros_iface_idle(wpa_supplicant *wpa_s)
00967 {
00968 if (wpa_s->ros_api)
00969 wpa_s->ros_api->ifaceIdle();
00970 }
00971
00972 void ros_scan_completed(wpa_supplicant *wpa_s, wpa_scan_results *scan_res)
00973 {
00974 if (wpa_s->ros_api)
00975 wpa_s->ros_api->scanCompleted(scan_res);
00976 }
00977
00978 void ros_do_work(int, void *, void *)
00979 {
00980 g_ros_api.doWork();
00981 }
00982
00983 void ros_assoc_failed(wpa_supplicant *wpa_s, const u8 *bssid, const char *reason)
00984 {
00985 if (wpa_s->ros_api)
00986 wpa_s->ros_api->assocFailed(bssid, reason);
00987 }
00988
00989 void ros_assoc_success(wpa_supplicant *wpa_s)
00990 {
00991 if (wpa_s->ros_api)
00992 wpa_s->ros_api->assocSucceeded();
00993 }
00994
00995 void ros_network_list_updated(wpa_supplicant *wpa_s)
00996 {
00997 if (wpa_s->ros_api)
00998 wpa_s->ros_api->publishNetworkList();
00999 }
01000 }