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 "um6/comms.h"
00044 #include "um6/registers.h"
00045 #include "um6/Reset.h"
00046 
00047 // Don't try to be too clever. Arrival of this message triggers
00048 // us to publish everything we have.
00049 const uint8_t TRIGGER_PACKET = UM6_TEMPERATURE;
00050 
00055 template<typename RegT>
00056 void configureVector3(um6::Comms* sensor, const um6::Accessor<RegT>& reg,
00057                       std::string param, std::string human_name)
00058 {
00059   if (reg.length != 3)
00060   {
00061     throw std::logic_error("configureVector3 may only be used with 3-field registers!");
00062   }
00063 
00064   if (ros::param::has(param))
00065   {
00066     double x, y, z;
00067     ros::param::get(param + "/x", x);
00068     ros::param::get(param + "/y", y);
00069     ros::param::get(param + "/z", z);
00070     ROS_INFO_STREAM("Configuring " << human_name << " to ("
00071                     << x << ", " << y << ", " << z << ")");
00072     reg.set_scaled(0, x);
00073     reg.set_scaled(1, y);
00074     reg.set_scaled(2, z);
00075     if (sensor->sendWaitAck(reg))
00076     {
00077       throw std::runtime_error("Unable to configure vector.");
00078     }
00079   }
00080 }
00081 
00086 template<typename RegT>
00087 void sendCommand(um6::Comms* sensor, const um6::Accessor<RegT>& reg, std::string human_name)
00088 {
00089   ROS_INFO_STREAM("Sending command: " << human_name);
00090   if (!sensor->sendWaitAck(reg))
00091   {
00092     throw std::runtime_error("Command to device failed.");
00093   }
00094 }
00095 
00096 
00101 void configureSensor(um6::Comms* sensor, ros::NodeHandle *private_nh)
00102 {
00103   um6::Registers r;
00104 
00105   // Enable outputs we need.
00106   const uint8_t UM6_BAUD_115200 = 0x5;
00107   uint32_t comm_reg = UM6_BROADCAST_ENABLED |
00108                       UM6_GYROS_PROC_ENABLED | UM6_ACCELS_PROC_ENABLED | UM6_MAG_PROC_ENABLED |
00109                       UM6_QUAT_ENABLED | UM6_EULER_ENABLED | UM6_COV_ENABLED | UM6_TEMPERATURE_ENABLED |
00110                       UM6_BAUD_115200 << UM6_BAUD_START_BIT;
00111   // set the broadcast rate of the device
00112   int rate;
00113   private_nh->param<int>("update_rate", rate, 20);
00114   if (rate < 20 || rate > 300)
00115   {
00116     ROS_WARN("Potentially unsupported update rate of %d", rate);
00117   }
00118   // converting from desired rate to broadcast_rate as defined in UM6 datasheet
00119   uint32_t rate_bits = (uint32_t) ((rate-20)*255.0/280.0);
00120   ROS_INFO("Setting update rate to %uHz", rate);
00121   comm_reg |= (rate_bits & UM6_SERIAL_RATE_MASK);
00122   r.communication.set(0, comm_reg);
00123   if (!sensor->sendWaitAck(r.communication))
00124   {
00125     throw std::runtime_error("Unable to set communication register.");
00126   }
00127 
00128   // Optionally disable mag and accel updates in the sensor's EKF.
00129   bool mag_updates, accel_updates;
00130   private_nh->param<bool>("mag_updates", mag_updates, true);
00131   private_nh->param<bool>("accel_updates", accel_updates, true);
00132   uint32_t misc_config_reg = UM6_QUAT_ESTIMATE_ENABLED;
00133   if (mag_updates)
00134   {
00135     misc_config_reg |= UM6_MAG_UPDATE_ENABLED;
00136   }
00137   else
00138   {
00139     ROS_WARN("Excluding magnetometer updates from EKF.");
00140   }
00141   if (accel_updates)
00142   {
00143     misc_config_reg |= UM6_ACCEL_UPDATE_ENABLED;
00144   }
00145   else
00146   {
00147     ROS_WARN("Excluding accelerometer updates from EKF.");
00148   }
00149   r.misc_config.set(0, misc_config_reg);
00150   if (!sensor->sendWaitAck(r.misc_config))
00151   {
00152     throw std::runtime_error("Unable to set misc config register.");
00153   }
00154 
00155   // Optionally disable the gyro reset on startup. A user might choose to do this
00156   // if there's an external process which can ascertain when the vehicle is stationary
00157   // and periodically call the /reset service.
00158   bool zero_gyros;
00159   private_nh->param<bool>("zero_gyros", zero_gyros, true);
00160   if (zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes");
00161 
00162   // Configurable vectors.
00163   configureVector3(sensor, r.mag_ref, "~mag_ref", "magnetic reference vector");
00164   configureVector3(sensor, r.accel_ref, "~accel_ref", "accelerometer reference vector");
00165   configureVector3(sensor, r.mag_bias, "~mag_bias", "magnetic bias vector");
00166   configureVector3(sensor, r.accel_bias, "~accel_bias", "accelerometer bias vector");
00167   configureVector3(sensor, r.gyro_bias, "~gyro_bias", "gyroscope bias vector");
00168 }
00169 
00170 
00171 bool handleResetService(um6::Comms* sensor,
00172                         const um6::Reset::Request& req, const um6::Reset::Response& resp)
00173 {
00174   um6::Registers r;
00175   if (req.zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes");
00176   if (req.reset_ekf) sendCommand(sensor, r.cmd_reset_ekf, "reset EKF");
00177   if (req.set_mag_ref) sendCommand(sensor, r.cmd_set_mag_ref, "set magnetometer reference");
00178   if (req.set_accel_ref) sendCommand(sensor, r.cmd_set_accel_ref, "set accelerometer reference");
00179   return true;
00180 }
00181 
00186 void publishMsgs(um6::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& imu_msg, bool tf_ned_to_enu)
00187 {
00188   static ros::Publisher imu_pub = imu_nh->advertise<sensor_msgs::Imu>("data", 1, false);
00189   static ros::Publisher mag_pub = imu_nh->advertise<geometry_msgs::Vector3Stamped>("mag", 1, false);
00190   static ros::Publisher rpy_pub = imu_nh->advertise<geometry_msgs::Vector3Stamped>("rpy", 1, false);
00191   static ros::Publisher temp_pub = imu_nh->advertise<std_msgs::Float32>("temperature", 1, false);
00192 
00193   if (imu_pub.getNumSubscribers() > 0)
00194   {
00195     // IMU reports a 4x4 wxyz covariance, ROS requires only 3x3 xyz.
00196     // NED -> ENU conversion req'd?
00197     imu_msg.orientation_covariance[0] = r.covariance.get_scaled(5);
00198     imu_msg.orientation_covariance[1] = r.covariance.get_scaled(6);
00199     imu_msg.orientation_covariance[2] = r.covariance.get_scaled(7);
00200     imu_msg.orientation_covariance[3] = r.covariance.get_scaled(9);
00201     imu_msg.orientation_covariance[4] = r.covariance.get_scaled(10);
00202     imu_msg.orientation_covariance[5] = r.covariance.get_scaled(11);
00203     imu_msg.orientation_covariance[6] = r.covariance.get_scaled(13);
00204     imu_msg.orientation_covariance[7] = r.covariance.get_scaled(14);
00205     imu_msg.orientation_covariance[8] = r.covariance.get_scaled(15);
00206 
00207     // NED -> ENU conversion (x = y, y = x, z = -z)
00208     if (tf_ned_to_enu)
00209     {
00210       imu_msg.orientation.x = r.quat.get_scaled(2);
00211       imu_msg.orientation.y = r.quat.get_scaled(1);
00212       imu_msg.orientation.z = -r.quat.get_scaled(3);
00213       imu_msg.orientation.w = r.quat.get_scaled(0);
00214 
00215       imu_msg.angular_velocity.x = r.gyro.get_scaled(1);
00216       imu_msg.angular_velocity.y = r.gyro.get_scaled(0);
00217       imu_msg.angular_velocity.z = -r.gyro.get_scaled(2);
00218 
00219       imu_msg.linear_acceleration.x = r.accel.get_scaled(1);
00220       imu_msg.linear_acceleration.y = r.accel.get_scaled(0);
00221       imu_msg.linear_acceleration.z = -r.accel.get_scaled(2);
00222     }
00223     else
00224     {
00225       imu_msg.orientation.w = r.quat.get_scaled(0);
00226       imu_msg.orientation.x = r.quat.get_scaled(1);
00227       imu_msg.orientation.y = r.quat.get_scaled(2);
00228       imu_msg.orientation.z = r.quat.get_scaled(3);
00229 
00230       imu_msg.angular_velocity.x = r.gyro.get_scaled(0);
00231       imu_msg.angular_velocity.y = r.gyro.get_scaled(1);
00232       imu_msg.angular_velocity.z = r.gyro.get_scaled(2);
00233 
00234       imu_msg.linear_acceleration.x = r.accel.get_scaled(0);
00235       imu_msg.linear_acceleration.y = r.accel.get_scaled(1);
00236       imu_msg.linear_acceleration.z = r.accel.get_scaled(2);
00237     }
00238 
00239     imu_pub.publish(imu_msg);
00240   }
00241 
00242   if (mag_pub.getNumSubscribers() > 0)
00243   {
00244     geometry_msgs::Vector3Stamped mag_msg;
00245     mag_msg.header = imu_msg.header;
00246 
00247     if (tf_ned_to_enu)
00248     {
00249       mag_msg.vector.x = r.mag.get_scaled(1);
00250       mag_msg.vector.y = r.mag.get_scaled(0);
00251       mag_msg.vector.z = -r.mag.get_scaled(2);
00252     }
00253     else
00254     {
00255       mag_msg.vector.x = r.mag.get_scaled(0);
00256       mag_msg.vector.y = r.mag.get_scaled(1);
00257       mag_msg.vector.z = r.mag.get_scaled(2);
00258     }
00259 
00260     mag_pub.publish(mag_msg);
00261   }
00262 
00263   if (rpy_pub.getNumSubscribers() > 0)
00264   {
00265     geometry_msgs::Vector3Stamped rpy_msg;
00266     rpy_msg.header = imu_msg.header;
00267 
00268     if (tf_ned_to_enu)
00269     {
00270       rpy_msg.vector.x = r.euler.get_scaled(1);
00271       rpy_msg.vector.y = r.euler.get_scaled(0);
00272       rpy_msg.vector.z = -r.euler.get_scaled(2);
00273     }
00274     else
00275     {
00276       rpy_msg.vector.x = r.euler.get_scaled(0);
00277       rpy_msg.vector.y = r.euler.get_scaled(1);
00278       rpy_msg.vector.z = r.euler.get_scaled(2);
00279     }
00280 
00281     rpy_pub.publish(rpy_msg);
00282   }
00283 
00284   if (temp_pub.getNumSubscribers() > 0)
00285   {
00286     std_msgs::Float32 temp_msg;
00287     temp_msg.data = r.temperature.get_scaled(0);
00288     temp_pub.publish(temp_msg);
00289   }
00290 }
00291 
00295 int main(int argc, char **argv)
00296 {
00297   ros::init(argc, argv, "um6_driver");
00298 
00299   // Load parameters from private node handle.
00300   std::string port;
00301   int32_t baud;
00302 
00303   ros::NodeHandle imu_nh("imu"), private_nh("~");
00304   private_nh.param<std::string>("port", port, "/dev/ttyUSB0");
00305   private_nh.param<int32_t>("baud", baud, 115200);
00306 
00307   serial::Serial ser;
00308   ser.setPort(port);
00309   ser.setBaudrate(baud);
00310   serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0);
00311   ser.setTimeout(to);
00312 
00313   sensor_msgs::Imu imu_msg;
00314   double linear_acceleration_stdev, angular_velocity_stdev;
00315   private_nh.param<std::string>("frame_id", imu_msg.header.frame_id, "imu_link");
00316   // Defaults obtained experimentally from hardware, no device spec exists
00317   private_nh.param<double>("linear_acceleration_stdev", linear_acceleration_stdev, 0.06);
00318   private_nh.param<double>("angular_velocity_stdev", angular_velocity_stdev, 0.005);
00319 
00320   double linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev;
00321   double angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev;
00322 
00323   // Enable converting from NED to ENU by default
00324   bool tf_ned_to_enu;
00325   private_nh.param<bool>("tf_ned_to_enu", tf_ned_to_enu, true);
00326 
00327   imu_msg.linear_acceleration_covariance[0] = linear_acceleration_cov;
00328   imu_msg.linear_acceleration_covariance[4] = linear_acceleration_cov;
00329   imu_msg.linear_acceleration_covariance[8] = linear_acceleration_cov;
00330 
00331   imu_msg.angular_velocity_covariance[0] = angular_velocity_cov;
00332   imu_msg.angular_velocity_covariance[4] = angular_velocity_cov;
00333   imu_msg.angular_velocity_covariance[8] = angular_velocity_cov;
00334 
00335   bool first_failure = true;
00336   while (ros::ok())
00337   {
00338     try
00339     {
00340       ser.open();
00341     }
00342     catch(const serial::IOException& e)
00343     {
00344       ROS_DEBUG("Unable to connect to port.");
00345     }
00346     if (ser.isOpen())
00347     {
00348       ROS_INFO("Successfully connected to serial port.");
00349       first_failure = true;
00350       try
00351       {
00352         um6::Comms sensor(&ser);
00353         configureSensor(&sensor, &private_nh);
00354         um6::Registers registers;
00355         ros::ServiceServer srv = imu_nh.advertiseService<um6::Reset::Request, um6::Reset::Response>(
00356                                    "reset", boost::bind(handleResetService, &sensor, _1, _2));
00357 
00358         while (ros::ok())
00359         {
00360           if (sensor.receive(&registers) == TRIGGER_PACKET)
00361           {
00362             // Triggered by arrival of final message in group.
00363             imu_msg.header.stamp = ros::Time::now();
00364             publishMsgs(registers, &imu_nh, imu_msg, tf_ned_to_enu);
00365             ros::spinOnce();
00366           }
00367         }
00368       }
00369       catch(const std::exception& e)
00370       {
00371         if (ser.isOpen()) ser.close();
00372         ROS_ERROR_STREAM(e.what());
00373         ROS_INFO("Attempting reconnection after error.");
00374         ros::Duration(1.0).sleep();
00375       }
00376     }
00377     else
00378     {
00379       ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device "
00380                            << port << ". Trying again every 1 second.");
00381       first_failure = false;
00382       ros::Duration(1.0).sleep();
00383     }
00384   }
00385 }


um6
Author(s): Mike Purvis
autogenerated on Fri Feb 12 2016 00:25:31