ros_mac_authentication.cpp
Go to the documentation of this file.
00001 
00014 #include <fstream>
00015 #include <openssl/sha.h>
00016 #include <ros/ros.h>
00017 #include <rosauth/Authentication.h>
00018 #include <sstream>
00019 #include <string>
00020 
00021 using namespace std;
00022 using namespace ros;
00023 
00029 #define SECRET_FILE_PARAM "/ros_mac_authentication/secret_file_location"
00030 
00035 #define MISSING_PARAMETER -1
00036 
00041 #define FILE_IO_ERROR -2
00042 
00047 #define INVALID_SECRET -3
00048 
00053 #define SECRET_LENGTH 16
00054 
00055 // the secret string used in the MAC
00056 string secret;
00057 
00058 bool authenticate(rosauth::Authentication::Request &req, rosauth::Authentication::Response &res)
00059 {
00060   // keep track of the current time
00061   Time t = Time::now();
00062   // clocks can be out of sync, check which comes later
00063   Duration *diff;
00064   if (req.t > t)
00065     diff = new Duration(req.t - t);
00066   else
00067     diff = new Duration(t - req.t);
00068   bool time_check = diff->sec < 5 && req.end > t;
00069   delete diff;
00070 
00071   // check if we pass the time requirement
00072   if (time_check)
00073   {
00074     // create the string to hash
00075     stringstream ss;
00076     ss << secret << req.client << req.dest << req.rand << req.t.sec << req.level << req.end.sec;
00077     string local_hash = ss.str();
00078 
00079     // check the request
00080     unsigned char sha512_hash[SHA512_DIGEST_LENGTH];
00081     SHA512((unsigned char *)local_hash.c_str(), local_hash.length(), sha512_hash);
00082 
00083     // convert to a hex string to compare
00084     char hex[SHA512_DIGEST_LENGTH * 2];
00085     for (int i = 0; i < SHA512_DIGEST_LENGTH; i++)
00086       sprintf(&hex[i * 2], "%02x", sha512_hash[i]);
00087 
00088     // an authenticated user must match the MAC string
00089     res.authenticated = (strcmp(hex, req.mac.c_str()) == 0);
00090   }
00091   else
00092     res.authenticated = false;
00093 
00094   return true;
00095 }
00096 
00104 int main(int argc, char **argv)
00105 {
00106   // initialize ROS and the node
00107   init(argc, argv, "ros_mac_authentication");
00108   NodeHandle node;
00109 
00110   // check if we have to check the secret file
00111   string file;
00112   if (!node.getParam(SECRET_FILE_PARAM, file))
00113   {
00114     ROS_ERROR("Parameter '%s' not found.", SECRET_FILE_PARAM);
00115     return MISSING_PARAMETER;
00116   }
00117   else
00118   {
00119     // try and load the file
00120     ifstream f;
00121     f.open(file.c_str(), ifstream::in);
00122     if (f.is_open())
00123     {
00124       // should be a 1 line file with the string
00125       getline(f, secret);
00126       f.close();
00127       // check the length of the secret
00128       if (secret.length() != SECRET_LENGTH)
00129       {
00130         ROS_ERROR("Secret string not of length %d.", SECRET_LENGTH);
00131         return INVALID_SECRET;
00132       }
00133       else
00134       {
00135         ServiceServer service = node.advertiseService("authenticate", authenticate);
00136         ROS_INFO("ROS Authentication Server Started");
00137         spin();
00138 
00139         return EXIT_SUCCESS;
00140       }
00141     }
00142     else
00143     {
00144       ROS_ERROR("Could not read from file '%s'", file.c_str());
00145       return FILE_IO_ERROR;
00146     }
00147   }
00148 }


rosauth
Author(s): Russell Toris
autogenerated on Thu Feb 25 2016 11:48:45