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";
00048
00049
00050
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
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);
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
00154 uint32_t misc_config_reg = 0;
00155
00156
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
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
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
00217
00218 if (tf_ned_to_enu)
00219 {
00220
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
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
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
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
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
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
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
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
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
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
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
00352 bool tf_ned_to_enu;
00353 private_nh.param<bool>("tf_ned_to_enu", tf_ned_to_enu, true);
00354
00355
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
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
00395 if (sensor.receive(®isters) == TRIGGER_PACKET)
00396 {
00397
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 }