main.cpp
Go to the documentation of this file.
00001 
00035 #include <string>
00036 
00037 #include "geometry_msgs/Vector3Stamped.h"
00038 #include "ros/ros.h"
00039 #include "sensor_msgs/Imu.h"
00040 #include "serial/serial.h"
00041 #include "std_msgs/Float32.h"
00042 #include "std_msgs/Header.h"
00043 #include "um7/comms.h"
00044 #include "um7/registers.h"
00045 #include "um7/Reset.h"
00046 
00047 const char VERSION[10] = "0.0.2";   // um7_driver version
00048 
00049 // Don't try to be too clever. Arrival of this message triggers
00050 // us to publish everything we have.
00051 const uint8_t TRIGGER_PACKET = DREG_EULER_PHI_THETA;
00052 
00057 template<typename RegT>
00058 void configureVector3(um7::Comms* sensor, const um7::Accessor<RegT>& reg,
00059                       std::string param, std::string human_name)
00060 {
00061   if (reg.length != 3)
00062   {
00063     throw std::logic_error("configureVector3 may only be used with 3-field registers!");
00064   }
00065 
00066   if (ros::param::has(param))
00067   {
00068     double x, y, z;
00069     ros::param::get(param + "/x", x);
00070     ros::param::get(param + "/y", y);
00071     ros::param::get(param + "/z", z);
00072     ROS_INFO_STREAM("Configuring " << human_name << " to ("
00073                     << x << ", " << y << ", " << z << ")");
00074     reg.set_scaled(0, x);
00075     reg.set_scaled(1, y);
00076     reg.set_scaled(2, z);
00077     if (sensor->sendWaitAck(reg))
00078     {
00079       throw std::runtime_error("Unable to configure vector.");
00080     }
00081   }
00082 }
00083 
00088 template<typename RegT>
00089 void sendCommand(um7::Comms* sensor, const um7::Accessor<RegT>& reg, std::string human_name)
00090 {
00091   ROS_INFO_STREAM("Sending command: " << human_name);
00092   if (!sensor->sendWaitAck(reg))
00093   {
00094     throw std::runtime_error("Command to device failed.");
00095   }
00096 }
00097 
00098 
00103 void configureSensor(um7::Comms* sensor, ros::NodeHandle *private_nh)
00104 {
00105   um7::Registers r;
00106 
00107   uint32_t comm_reg = (BAUD_115200 << COM_BAUD_START);
00108   r.communication.set(0, comm_reg);
00109   if (!sensor->sendWaitAck(r.comrate2))
00110   {
00111     throw std::runtime_error("Unable to set CREG_COM_SETTINGS.");
00112   }
00113 
00114   // set the broadcast rate of the device
00115   int rate;
00116   private_nh->param<int>("update_rate", rate, 20);
00117   if (rate < 20 || rate > 255)
00118   {
00119     ROS_WARN("Potentially unsupported update rate of %d", rate);
00120   }
00121 
00122   uint32_t rate_bits = static_cast<uint32_t>(rate);
00123   ROS_INFO("Setting update rate to %uHz", rate);
00124   uint32_t raw_rate = (rate_bits << RATE2_ALL_RAW_START);
00125   r.comrate2.set(0, raw_rate);
00126   if (!sensor->sendWaitAck(r.comrate2))
00127   {
00128     throw std::runtime_error("Unable to set CREG_COM_RATES2.");
00129   }
00130 
00131   uint32_t proc_rate = (rate_bits << RATE4_ALL_PROC_START);
00132   r.comrate4.set(0, proc_rate);
00133   if (!sensor->sendWaitAck(r.comrate4))
00134   {
00135     throw std::runtime_error("Unable to set CREG_COM_RATES4.");
00136   }
00137 
00138   uint32_t misc_rate = (rate_bits << RATE5_EULER_START) | (rate_bits << RATE5_QUAT_START);
00139   r.comrate5.set(0, misc_rate);
00140   if (!sensor->sendWaitAck(r.comrate5))
00141   {
00142     throw std::runtime_error("Unable to set CREG_COM_RATES5.");
00143   }
00144 
00145   uint32_t health_rate = (5 << RATE6_HEALTH_START);  // note:  5 gives 2 hz rate
00146   r.comrate6.set(0, health_rate);
00147   if (!sensor->sendWaitAck(r.comrate6))
00148   {
00149     throw std::runtime_error("Unable to set CREG_COM_RATES6.");
00150   }
00151 
00152 
00153   // Options available using parameters)
00154   uint32_t misc_config_reg = 0;  // initialize all options off
00155 
00156   // Optionally disable mag updates in the sensor's EKF.
00157   bool mag_updates;
00158   private_nh->param<bool>("mag_updates", mag_updates, true);
00159   if (mag_updates)
00160   {
00161     misc_config_reg |= MAG_UPDATES_ENABLED;
00162   }
00163   else
00164   {
00165     ROS_WARN("Excluding magnetometer updates from EKF.");
00166   }
00167 
00168   // Optionally enable quaternion mode .
00169   bool quat_mode;
00170   private_nh->param<bool>("quat_mode", quat_mode, true);
00171   if (quat_mode)
00172   {
00173     misc_config_reg |= QUATERNION_MODE_ENABLED;
00174   }
00175   else
00176   {
00177     ROS_WARN("Excluding quaternion mode.");
00178   }
00179 
00180   r.misc_config.set(0, misc_config_reg);
00181   if (!sensor->sendWaitAck(r.misc_config))
00182   {
00183     throw std::runtime_error("Unable to set CREG_MISC_SETTINGS.");
00184   }
00185 
00186   // Optionally disable performing a zero gyros command on driver startup.
00187   bool zero_gyros;
00188   private_nh->param<bool>("zero_gyros", zero_gyros, true);
00189   if (zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes");
00190 }
00191 
00192 
00193 bool handleResetService(um7::Comms* sensor,
00194     const um7::Reset::Request& req, const um7::Reset::Response& resp)
00195 {
00196   um7::Registers r;
00197   if (req.zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes");
00198   if (req.reset_ekf) sendCommand(sensor, r.cmd_reset_ekf, "reset EKF");
00199   if (req.set_mag_ref) sendCommand(sensor, r.cmd_set_mag_ref, "set magnetometer reference");
00200   return true;
00201 }
00202 
00207 void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& imu_msg, bool tf_ned_to_enu)
00208 {
00209   static ros::Publisher imu_pub = imu_nh->advertise<sensor_msgs::Imu>("data", 1, false);
00210   static ros::Publisher mag_pub = imu_nh->advertise<geometry_msgs::Vector3Stamped>("mag", 1, false);
00211   static ros::Publisher rpy_pub = imu_nh->advertise<geometry_msgs::Vector3Stamped>("rpy", 1, false);
00212   static ros::Publisher temp_pub = imu_nh->advertise<std_msgs::Float32>("temperature", 1, false);
00213 
00214   if (imu_pub.getNumSubscribers() > 0)
00215   {
00216     // body-fixed frame NED to ENU: (x y z)->(x -y -z) or (w x y z)->(x -y -z w)
00217     // world frame      NED to ENU: (x y z)->(y  x -z) or (w x y z)->(y  x -z w)
00218     if (tf_ned_to_enu)
00219     {
00220       // world frame
00221       imu_msg.orientation.w =  r.quat.get_scaled(2);
00222       imu_msg.orientation.x =  r.quat.get_scaled(1);
00223       imu_msg.orientation.y = -r.quat.get_scaled(3);
00224       imu_msg.orientation.z =  r.quat.get_scaled(0);
00225 
00226       // body-fixed frame
00227       imu_msg.angular_velocity.x =  r.gyro.get_scaled(0);
00228       imu_msg.angular_velocity.y = -r.gyro.get_scaled(1);
00229       imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);
00230 
00231       // body-fixed frame
00232       imu_msg.linear_acceleration.x =  r.accel.get_scaled(0);
00233       imu_msg.linear_acceleration.y = -r.accel.get_scaled(1);
00234       imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
00235     }
00236     else
00237     {
00238       imu_msg.orientation.w = r.quat.get_scaled(0);
00239       imu_msg.orientation.x = r.quat.get_scaled(1);
00240       imu_msg.orientation.y = r.quat.get_scaled(2);
00241       imu_msg.orientation.z = r.quat.get_scaled(3);
00242 
00243       imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
00244       imu_msg.angular_velocity.y = r.gyro.get_scaled(1);
00245       imu_msg.angular_velocity.z = r.gyro.get_scaled(2);
00246 
00247       imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
00248       imu_msg.linear_acceleration.y = r.accel.get_scaled(1);
00249       imu_msg.linear_acceleration.z = r.accel.get_scaled(2);
00250     }
00251 
00252     imu_pub.publish(imu_msg);
00253   }
00254 
00255   // Magnetometer.  transform to ROS axes
00256   if (mag_pub.getNumSubscribers() > 0)
00257   {
00258     geometry_msgs::Vector3Stamped mag_msg;
00259     mag_msg.header = imu_msg.header;
00260 
00261     if (tf_ned_to_enu)
00262     {
00263       // world frame
00264       mag_msg.vector.x =  r.mag.get_scaled(1);
00265       mag_msg.vector.y =  r.mag.get_scaled(0);
00266       mag_msg.vector.z = -r.mag.get_scaled(2);
00267     }
00268     else
00269     {
00270       mag_msg.vector.x = r.mag.get_scaled(0);
00271       mag_msg.vector.y = r.mag.get_scaled(1);
00272       mag_msg.vector.z = r.mag.get_scaled(2);
00273     }
00274 
00275     mag_pub.publish(mag_msg);
00276   }
00277 
00278   // Euler attitudes.  transform to ROS axes
00279   if (rpy_pub.getNumSubscribers() > 0)
00280   {
00281     geometry_msgs::Vector3Stamped rpy_msg;
00282     rpy_msg.header = imu_msg.header;
00283 
00284     if (tf_ned_to_enu)
00285     {
00286       // world frame
00287       rpy_msg.vector.x =  r.euler.get_scaled(1);
00288       rpy_msg.vector.y =  r.euler.get_scaled(0);
00289       rpy_msg.vector.z = -r.euler.get_scaled(2);
00290     }
00291     else
00292     {
00293       rpy_msg.vector.x = r.euler.get_scaled(0);
00294       rpy_msg.vector.y = r.euler.get_scaled(1);
00295       rpy_msg.vector.z = r.euler.get_scaled(2);
00296     }
00297 
00298     rpy_pub.publish(rpy_msg);
00299   }
00300 
00301   // Temperature
00302   if (temp_pub.getNumSubscribers() > 0)
00303   {
00304     std_msgs::Float32 temp_msg;
00305     temp_msg.data = r.temperature.get_scaled(0);
00306     temp_pub.publish(temp_msg);
00307   }
00308 }
00309 
00313 int main(int argc, char **argv)
00314 {
00315   ros::init(argc, argv, "um7_driver");
00316 
00317   // Load parameters from private node handle.
00318   std::string port;
00319   int32_t baud;
00320 
00321   ros::NodeHandle imu_nh("imu"), private_nh("~");
00322   private_nh.param<std::string>("port", port, "/dev/ttyUSB0");
00323   private_nh.param<int32_t>("baud", baud, 115200);
00324 
00325   serial::Serial ser;
00326   ser.setPort(port);
00327   ser.setBaudrate(baud);
00328   serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0);
00329   ser.setTimeout(to);
00330 
00331   sensor_msgs::Imu imu_msg;
00332   double linear_acceleration_stdev, angular_velocity_stdev;
00333   private_nh.param<std::string>("frame_id", imu_msg.header.frame_id, "imu_link");
00334   // Defaults obtained experimentally from hardware, no device spec exists
00335   private_nh.param<double>("linear_acceleration_stdev", linear_acceleration_stdev, (4.0 * 1e-3f * 9.80665));
00336   private_nh.param<double>("angular_velocity_stdev", angular_velocity_stdev, (0.06 * 3.14159 / 180.0));
00337 
00338   double linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev;
00339   double angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev;
00340 
00341   // From the UM7 datasheet for the dynamic accuracy from the EKF.
00342   double orientation_x_stdev, orientation_y_stdev, orientation_z_stdev;
00343   private_nh.param<double>("orientation_x_stdev", orientation_x_stdev, (3.0 * 3.14159 / 180.0));
00344   private_nh.param<double>("orientation_y_stdev", orientation_y_stdev, (3.0 * 3.14159 / 180.0));
00345   private_nh.param<double>("orientation_z_stdev", orientation_z_stdev, (5.0 * 3.14159 / 180.0));
00346 
00347   double orientation_x_covar = orientation_x_stdev * orientation_x_stdev;
00348   double orientation_y_covar = orientation_y_stdev * orientation_y_stdev;
00349   double orientation_z_covar = orientation_z_stdev * orientation_z_stdev;
00350 
00351   // Enable converting from NED to ENU by default
00352   bool tf_ned_to_enu;
00353   private_nh.param<bool>("tf_ned_to_enu", tf_ned_to_enu, true);
00354 
00355   // These values do not need to be converted
00356   imu_msg.linear_acceleration_covariance[0] = linear_acceleration_cov;
00357   imu_msg.linear_acceleration_covariance[4] = linear_acceleration_cov;
00358   imu_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
00359 
00360   imu_msg.angular_velocity_covariance[0] = angular_velocity_cov;
00361   imu_msg.angular_velocity_covariance[4] = angular_velocity_cov;
00362   imu_msg.angular_velocity_covariance[8] = angular_velocity_cov;
00363 
00364   imu_msg.orientation_covariance[0] = orientation_x_covar;
00365   imu_msg.orientation_covariance[4] = orientation_y_covar;
00366   imu_msg.orientation_covariance[8] = orientation_z_covar;
00367 
00368   // Real Time Loop
00369   bool first_failure = true;
00370   while (ros::ok())
00371   {
00372     try
00373     {
00374       ser.open();
00375     }
00376     catch (const serial::IOException& e)
00377     {
00378         ROS_WARN("um7_driver was unable to connect to port %s.", port.c_str());
00379     }
00380     if (ser.isOpen())
00381     {
00382       ROS_INFO("um7_driver successfully connected to serial port %s.", port.c_str());
00383       first_failure = true;
00384       try
00385       {
00386         um7::Comms sensor(&ser);
00387         configureSensor(&sensor, &private_nh);
00388         um7::Registers registers;
00389         ros::ServiceServer srv = imu_nh.advertiseService<um7::Reset::Request, um7::Reset::Response>(
00390             "reset", boost::bind(handleResetService, &sensor, _1, _2));
00391 
00392         while (ros::ok())
00393         {
00394           // triggered by arrival of last message packet
00395           if (sensor.receive(&registers) == TRIGGER_PACKET)
00396           {
00397             // Triggered by arrival of final message in group.
00398             imu_msg.header.stamp = ros::Time::now();
00399             publishMsgs(registers, &imu_nh, imu_msg, tf_ned_to_enu);
00400             ros::spinOnce();
00401           }
00402         }
00403       }
00404       catch(const std::exception& e)
00405       {
00406         if (ser.isOpen()) ser.close();
00407         ROS_ERROR_STREAM(e.what());
00408         ROS_INFO("Attempting reconnection after error.");
00409         ros::Duration(1.0).sleep();
00410       }
00411     }
00412     else
00413     {
00414       ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device "
00415                            << port << ". Trying again every 1 second.");
00416       first_failure = false;
00417       ros::Duration(1.0).sleep();
00418     }
00419   }
00420 }


um7
Author(s): Mike Purvis , Alex Brown
autogenerated on Sat Jan 28 2017 03:34:13