orientus_node.cpp
Go to the documentation of this file.
00001 #include <stdlib.h>
00002 #include <stdio.h>
00003 #include <stdint.h>
00004 #include <math.h>
00005 
00006 #include <boost/asio.hpp>
00007 
00008 #include "orientus_sdk_c/an_packet_protocol.h"
00009 #include "orientus_sdk_c/orientus_packets.h"
00010 
00011 #include "ros/ros.h"
00012 #include "sensor_msgs/Imu.h"
00013 #include "sensor_msgs/MagneticField.h"
00014 #include "sensor_msgs/Temperature.h"
00015 #include "geometry_msgs/Quaternion.h"
00016 #include "diagnostic_updater/diagnostic_updater.h"
00017 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00018 
00019 class OrientusNode {
00020 private:
00021   ros::NodeHandle nh_;
00022   ros::NodeHandle pnh_;
00023   ros::Rate loop_rate_;
00024   std::string frame_id_;
00025   ros::Publisher imu_pub_;
00026   ros::Publisher imu_raw_pub_;
00027   ros::Publisher magnetic_field_pub_;
00028   ros::Publisher temperature_pub_;
00029   diagnostic_updater::Updater diagnostic_;
00030 
00031   boost::asio::io_service io_service_;
00032   std::string port_name_;
00033   boost::asio::serial_port port_;
00034   an_decoder_t an_decoder_;
00035 
00036   quaternion_orientation_standard_deviation_packet_t quaternion_std_packet_;
00037   acceleration_packet_t acceleration_packet_;
00038   quaternion_orientation_packet_t quaternion_packet_;
00039   angular_velocity_packet_t angular_velocity_packet_;
00040   raw_sensors_packet_t raw_sensors_packet_;
00041   status_packet_t status_packet_;
00042   running_time_packet_t running_time_packet_;
00043   device_information_packet_t device_information_packet_;
00044   bool quaternion_orientation_std_received_;
00045   bool quaternion_orientation_received_;
00046   bool acceleration_received_;
00047   bool angular_velocity_received_;
00048   bool raw_sensors_received_;
00049   bool status_received_;
00050   bool running_time_received_;
00051   bool device_information_received_;
00052 
00053 public:
00054   OrientusNode(ros::NodeHandle nh, ros::NodeHandle pnh)
00055     : nh_(nh), pnh_(pnh), loop_rate_(100),
00056       diagnostic_(nh_, pnh_), port_(io_service_){
00057 
00058     pnh_.param("port", port_name_, std::string("/dev/ttyUSB0"));
00059     pnh_.param("frame_id", frame_id_, std::string("imu"));
00060 
00061     port_.open(port_name_);
00062     port_.set_option(boost::asio::serial_port_base::baud_rate(115200));
00063 
00064     an_packet_t *an_packet;
00065     // Request device information
00066     an_packet = encode_request_packet(packet_id_device_information);
00067     an_packet_encode_and_send(an_packet);
00068     an_packet_free(&an_packet);
00069 
00070     // Setup packet rate timer
00071     packet_timer_period_packet_t packet_timer_period_packet;
00072     packet_timer_period_packet.permanent = 0;
00073     packet_timer_period_packet.packet_timer_period = 1000; // 1 ms
00074     an_packet = encode_packet_timer_period_packet(&packet_timer_period_packet);
00075     an_packet_encode_and_send(an_packet);
00076     an_packet_free(&an_packet);
00077 
00078     // Configure packet rates
00079     packet_periods_packet_t packet_periods_packet;
00080     packet_periods_packet.permanent = 0;
00081     packet_periods_packet.clear_existing_packets = 1;
00082     packet_periods_packet.packet_periods[0].packet_id = packet_id_quaternion_orientation_standard_deviation;
00083     packet_periods_packet.packet_periods[0].period = 20; // 50 Hz
00084     packet_periods_packet.packet_periods[1].packet_id = packet_id_quaternion_orientation;
00085     packet_periods_packet.packet_periods[1].period = 20; // 50 Hz
00086     packet_periods_packet.packet_periods[2].packet_id = packet_id_acceleration;
00087     packet_periods_packet.packet_periods[2].period = 20; // 50 Hz
00088     packet_periods_packet.packet_periods[3].packet_id = packet_id_angular_velocity;
00089     packet_periods_packet.packet_periods[3].period = 20; // 50 Hz
00090     packet_periods_packet.packet_periods[4].packet_id = packet_id_raw_sensors;
00091     packet_periods_packet.packet_periods[4].period = 20; // 50 Hz
00092 
00093     packet_periods_packet.packet_periods[5].packet_id = packet_id_status;
00094     packet_periods_packet.packet_periods[5].period = 200; // 5 Hz
00095     packet_periods_packet.packet_periods[6].packet_id = packet_id_running_time;
00096     packet_periods_packet.packet_periods[6].period = 200; // 5 Hz
00097 
00098     packet_periods_packet.packet_periods[7].packet_id = 0;
00099     an_packet = encode_packet_periods_packet(&packet_periods_packet);
00100     an_packet_encode_and_send(an_packet);
00101     an_packet_free(&an_packet);
00102 
00103 
00104     // Setup ROS topic
00105     imu_pub_ = nh.advertise<sensor_msgs::Imu>("imu/data", 10);
00106     imu_raw_pub_ = nh.advertise<sensor_msgs::Imu>("imu/raw", 10);
00107     magnetic_field_pub_ = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10);
00108     temperature_pub_ = nh.advertise<sensor_msgs::Temperature>("imu/temp", 10);
00109 
00110 
00111     // Setup diagnostics
00112     diagnostic_.setHardwareID("orientus");
00113     diagnostic_.add("IMU Status", this, &OrientusNode::deviceStatus);
00114     diagnostic_.add("Accelerometer Status", this, &OrientusNode::accelerometerStatus);
00115     diagnostic_.add("Gyroscope Status", this, &OrientusNode::gyroscopeStatus);
00116     diagnostic_.add("Magnetometer Status", this, &OrientusNode::magnetometerStatus);
00117     diagnostic_.add("Temperature Status", this, &OrientusNode::temperatureStatus);
00118     diagnostic_.add("Voltage Status", this, &OrientusNode::voltageStatus);
00119     diagnostic_.add("Filter Status", this, &OrientusNode::filterStatus);
00120 
00121 
00122     an_decoder_initialise(&an_decoder_);
00123 
00124     quaternion_orientation_std_received_ = false;
00125     quaternion_orientation_received_ = false;
00126     acceleration_received_ = false;
00127     raw_sensors_received_ = false;
00128     status_received_ = false;
00129     running_time_received_ = false;
00130     device_information_received_ = false;
00131   }
00132   void spin() {
00133     while (ros::ok()) {
00134       while(receive_next_packet()){
00135         if(quaternion_orientation_std_received_ && quaternion_orientation_received_
00136            && acceleration_received_ && angular_velocity_received_ && raw_sensors_received_) {
00137           publish_imu_msg();
00138           quaternion_orientation_std_received_ = false;
00139           quaternion_orientation_received_ = false;
00140           acceleration_received_ = false;
00141           angular_velocity_received_ = false;
00142         }
00143         if(raw_sensors_received_) {
00144           publish_imu_raw_msg();
00145           publish_magnetics_msg();
00146           publish_temperature_msg();
00147           raw_sensors_received_ = false;
00148         }
00149 
00150         if(status_received_ && running_time_received_ && device_information_received_){
00151           diagnostic_.update();
00152           status_received_ = false;
00153           running_time_received_ = false;
00154         }
00155       }
00156 
00157       ros::spinOnce();
00158       loop_rate_.sleep();
00159     }
00160   }
00161 
00162 private:
00163   int an_packet_encode_and_send(an_packet_t* packet) {
00164     an_packet_encode(packet);
00165     // need cast to prevent autosizing of packet
00166     return boost::asio::write(port_, boost::asio::buffer((const void*)an_packet_pointer(packet), an_packet_size(packet)));
00167   }
00168   bool receive_next_packet(){
00169     an_packet_t *an_packet;
00170     int bytes_received;
00171 
00172     if ((bytes_received = port_.read_some(boost::asio::buffer(an_decoder_pointer(&an_decoder_), an_decoder_size(&an_decoder_)))) > 0) {
00173       /* increment the decode buffer length by the number of bytes received */
00174       an_decoder_increment(&an_decoder_, bytes_received);
00175 
00176       /* decode all the packets in the buffer */
00177       if((an_packet = an_packet_decode(&an_decoder_)) != NULL) {
00178         if(an_packet->id == packet_id_acknowledge) {
00179           acknowledge_packet_t acknowledge_packet;
00180           if(decode_acknowledge_packet(&acknowledge_packet, an_packet) != 0) {
00181             ROS_WARN("Acknowledge packet decode failure");
00182           }
00183           else if(acknowledge_packet.acknowledge_result){
00184             ROS_WARN("Acknowledge Failure: %d", acknowledge_packet.acknowledge_result);
00185           }
00186         }
00187         else if(an_packet->id == packet_id_status) {
00188           if(decode_status_packet(&status_packet_, an_packet) != 0) {
00189             ROS_WARN("Status packet decode failure");
00190           }
00191           else{
00192             status_received_ = true;
00193           }
00194         }
00195         else if(an_packet->id == packet_id_running_time) {
00196           if(decode_running_time_packet(&running_time_packet_, an_packet) != 0) {
00197             ROS_WARN("Running time packet decode failure");
00198           }
00199           else{
00200             running_time_received_ = true;
00201           }
00202         }
00203         else if(an_packet->id == packet_id_device_information) {
00204           if(decode_device_information_packet(&device_information_packet_, an_packet) != 0) {
00205             ROS_WARN("Device information decode failure");
00206           }
00207           else{
00208             device_information_received_ = true;
00209           }
00210         }
00211         else if(an_packet->id == packet_id_quaternion_orientation_standard_deviation) {
00212           if(decode_quaternion_orientation_standard_deviation_packet(&quaternion_std_packet_, an_packet) != 0) {
00213             ROS_WARN("Quaternion orientation standard deviation packet decode failure");
00214           }
00215           else{
00216             quaternion_orientation_std_received_ = true;
00217           }
00218         }
00219         else if(an_packet->id == packet_id_quaternion_orientation) {
00220           if(decode_quaternion_orientation_packet(&quaternion_packet_, an_packet) != 0) {
00221             ROS_WARN("Quaternion packet decode failure");
00222           }
00223           else {
00224             quaternion_orientation_received_ = true;
00225           }
00226         }
00227         else if(an_packet->id == packet_id_acceleration) {
00228           if(decode_acceleration_packet(&acceleration_packet_, an_packet) != 0) {
00229             ROS_WARN("Acceleration packet decode failure");
00230           }
00231           else {
00232             acceleration_received_ = true;
00233           }
00234         }
00235         else if(an_packet->id == packet_id_angular_velocity) {
00236           if(decode_angular_velocity_packet(&angular_velocity_packet_, an_packet) != 0) {
00237             ROS_WARN("Angular velocity packet decode failure");
00238           }
00239           else {
00240             angular_velocity_received_ = true;
00241           }
00242         }
00243         else if(an_packet->id == packet_id_raw_sensors) {
00244           if(decode_raw_sensors_packet(&raw_sensors_packet_, an_packet) != 0) {
00245             ROS_WARN("Raw sensors packet decode failure");
00246           }
00247           else {
00248             raw_sensors_received_ = true;
00249           }
00250         }
00251         else {
00252           ROS_WARN("Unknown packet id: %d", an_packet->id);
00253         }
00254 
00255         an_packet_free(&an_packet);
00256       }
00257     }
00258   }
00259 
00260   void deviceStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00261     if(status_packet_.system_status.b.system_failure)
00262       status.summary(diagnostic_updater::DiagnosticStatusWrapper::ERROR, "System Failure");
00263     else if(status_packet_.system_status.b.serial_port_overflow_alarm)
00264       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Serial Port Overflow");
00265     else
00266       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "IMU is OK");
00267 
00268     status.add("Device", port_name_);
00269     status.add("TF frame", frame_id_);
00270     double running_time = (1.0e-6) * running_time_packet_.microseconds + running_time_packet_.seconds;
00271     status.add("Running time", running_time);
00272     status.add("Device ID", device_information_packet_.device_id);
00273 
00274     std::ostringstream software_version_stream;
00275     software_version_stream << std::fixed << std::setprecision(3);
00276     software_version_stream << device_information_packet_.software_version/1000.0;
00277     status.add("Software Version", software_version_stream.str());
00278 
00279     std::ostringstream hardware_revision_stream;
00280     hardware_revision_stream << std::fixed << std::setprecision(3);
00281     hardware_revision_stream << device_information_packet_.hardware_revision/1000.0;
00282     status.add("Hardware Revision", hardware_revision_stream.str());
00283 
00284     std::stringstream serial_number_stream;
00285     serial_number_stream << std::hex << std::setfill('0') << std::setw(8);
00286     serial_number_stream << device_information_packet_.serial_number[0];
00287     serial_number_stream << device_information_packet_.serial_number[1];
00288     serial_number_stream << device_information_packet_.serial_number[2];
00289     status.add("Serial Number", serial_number_stream.str());
00290   }
00291   void accelerometerStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00292     if(status_packet_.system_status.b.accelerometer_sensor_failure)
00293       status.summary(diagnostic_updater::DiagnosticStatusWrapper::ERROR, "Accelerometer Sensor Failure");
00294     else if(status_packet_.system_status.b.accelerometer_over_range)
00295       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Accelerometer Sensor Over Range");
00296     else
00297       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Accelerometer OK");
00298   }
00299   void gyroscopeStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00300     if(status_packet_.system_status.b.gyroscope_sensor_failure)
00301       status.summary(diagnostic_updater::DiagnosticStatusWrapper::ERROR, "Gyroscope Sensor Failure");
00302     else if(status_packet_.system_status.b.gyroscope_over_range)
00303       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Gyroscope Sensor Over Range");
00304     else
00305       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Gyroscope OK");
00306   }
00307   void magnetometerStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00308     if(status_packet_.system_status.b.magnetometer_sensor_failure)
00309       status.summary(diagnostic_updater::DiagnosticStatusWrapper::ERROR, "Magnetometer Sensor Failure");
00310     else if(status_packet_.system_status.b.magnetometer_over_range)
00311       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Magnetometer Sensor Over Range");
00312     else
00313       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Magnetometer OK");
00314   }
00315   void temperatureStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00316     if(status_packet_.system_status.b.minimum_temperature_alarm)
00317       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Minimum Temperature Alarm");
00318     else if(status_packet_.system_status.b.maximum_temperature_alarm)
00319       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Maximum Temperature Alarm");
00320     else
00321       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Temperature OK");
00322   }
00323   void voltageStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00324     if(status_packet_.system_status.b.low_voltage_alarm)
00325       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "Low Voltage");
00326     else if(status_packet_.system_status.b.high_voltage_alarm)
00327       status.summary(diagnostic_updater::DiagnosticStatusWrapper::WARN, "High Voltage");
00328     else
00329       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Voltage OK");
00330   }
00331   void filterStatus(diagnostic_updater::DiagnosticStatusWrapper &status) {
00332     if(status_packet_.filter_status.b.orientation_filter_initialised)
00333       status.summary(diagnostic_updater::DiagnosticStatusWrapper::OK, "Filter OK");
00334     else
00335       status.summary(diagnostic_updater::DiagnosticStatusWrapper::ERROR, "Filter Not Initialised");
00336 
00337     status.add("Heading Initialised", (bool)status_packet_.filter_status.b.heading_initialised);
00338     status.add("Magnetometers Enabled", (bool)status_packet_.filter_status.b.magnetometers_enabled);
00339     status.add("Velocity Heading Enabled", (bool)status_packet_.filter_status.b.velocity_heading_enabled);
00340     status.add("External Position Active", (bool)status_packet_.filter_status.b.external_position_active);
00341     status.add("External Velocity Active", (bool)status_packet_.filter_status.b.external_velocity_active);
00342     status.add("External Heading Active", (bool)status_packet_.filter_status.b.external_heading_active);
00343   }
00344 
00345   void publish_imu_msg() {
00346     sensor_msgs::Imu imu_msg;
00347     geometry_msgs::Quaternion quaternion_msg;
00348     float orientation_covariance[9] = {pow((quaternion_std_packet_.standard_deviation[0]), 2.0), 0, 0,
00349                                        0, pow((quaternion_std_packet_.standard_deviation[1]), 2.0), 0,
00350                                        0, 0, pow((quaternion_std_packet_.standard_deviation[2]), 2.0)};
00351     geometry_msgs::Vector3 angular_velocity;
00352 
00353     geometry_msgs::Vector3 linear_acceleration;
00354 
00355     imu_msg.header.stamp = ros::Time::now();
00356     imu_msg.header.frame_id = frame_id_;
00357 
00358     imu_msg.orientation.x = quaternion_packet_.orientation[0];
00359     imu_msg.orientation.y = quaternion_packet_.orientation[1];
00360     imu_msg.orientation.z = quaternion_packet_.orientation[2];
00361     imu_msg.orientation.w = quaternion_packet_.orientation[3];
00362     imu_msg.orientation_covariance[0] = orientation_covariance[0];
00363     imu_msg.orientation_covariance[4] = orientation_covariance[4];
00364     imu_msg.orientation_covariance[8] = orientation_covariance[8];
00365 
00366     imu_msg.angular_velocity.x = angular_velocity_packet_.angular_velocity[0];
00367     imu_msg.angular_velocity.y = angular_velocity_packet_.angular_velocity[1];
00368     imu_msg.angular_velocity.z = angular_velocity_packet_.angular_velocity[2];
00369     imu_msg.angular_velocity_covariance[0] = -1;
00370 
00371     imu_msg.linear_acceleration.x = acceleration_packet_.acceleration[0];
00372     imu_msg.linear_acceleration.y = acceleration_packet_.acceleration[1];
00373     imu_msg.linear_acceleration.z = acceleration_packet_.acceleration[2];
00374     imu_msg.linear_acceleration_covariance[0] = -1;
00375 
00376     imu_pub_.publish(imu_msg);
00377   }
00378   void publish_imu_raw_msg() {
00379     sensor_msgs::Imu imu_msg;
00380     geometry_msgs::Vector3 angular_velocity;
00381     geometry_msgs::Vector3 linear_acceleration;
00382 
00383     imu_msg.header.stamp = ros::Time::now();
00384     imu_msg.header.frame_id = frame_id_;
00385 
00386     imu_msg.orientation_covariance[0] = -1;
00387 
00388     imu_msg.angular_velocity.x = raw_sensors_packet_.gyroscopes[0];
00389     imu_msg.angular_velocity.y = raw_sensors_packet_.gyroscopes[1];
00390     imu_msg.angular_velocity.z = raw_sensors_packet_.gyroscopes[2];
00391     imu_msg.angular_velocity_covariance[0] = -1;
00392 
00393     imu_msg.linear_acceleration.x = raw_sensors_packet_.accelerometers[0];
00394     imu_msg.linear_acceleration.y = raw_sensors_packet_.accelerometers[1];
00395     imu_msg.linear_acceleration.z = raw_sensors_packet_.accelerometers[2];
00396     imu_msg.linear_acceleration_covariance[0] = -1;
00397 
00398     imu_raw_pub_.publish(imu_msg);
00399   }
00400   void publish_magnetics_msg() {
00401     sensor_msgs::MagneticField magnetic_field_msg;
00402 
00403     magnetic_field_msg.header.stamp = ros::Time::now();
00404     magnetic_field_msg.header.frame_id = frame_id_;
00405 
00406     magnetic_field_msg.magnetic_field.x = raw_sensors_packet_.magnetometers[0] * (1.0e-7);
00407     magnetic_field_msg.magnetic_field.y = raw_sensors_packet_.magnetometers[1] * (1.0e-7);
00408     magnetic_field_msg.magnetic_field.z = raw_sensors_packet_.magnetometers[2] * (1.0e-7);
00409 
00410     magnetic_field_pub_.publish(magnetic_field_msg);
00411   }
00412   void publish_temperature_msg() {
00413     sensor_msgs::Temperature temp_msg;
00414 
00415     temp_msg.header.stamp = ros::Time::now();
00416     temp_msg.header.frame_id = frame_id_;
00417 
00418     temp_msg.temperature = raw_sensors_packet_.imu_temperature;
00419 
00420     temperature_pub_.publish(temp_msg);
00421   }
00422 
00423 };
00424 
00425 int main(int argc, char *argv[]) {
00426   ros::init(argc, argv, "orientus_node");
00427   ros::NodeHandle nh;
00428   ros::NodeHandle pnh("~");
00429 
00430   try {
00431     OrientusNode node(nh, pnh);
00432     node.spin();
00433   } catch(std::exception& e){
00434     ROS_FATAL_STREAM("Exception thrown: " << e.what());
00435   }
00436 
00437   return 0;
00438 
00439 }


orientus_driver
Author(s): Nick Otero, Mitchell Wills
autogenerated on Wed Aug 26 2015 15:12:18