bmg160_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2012, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00037 //\Author Joshua Vasquez and Philip Roan, Robert Bosch LLC
00038 
00039 // ROS headers
00040 #include <ros/ros.h>
00041 #include <ros/time.h>
00042 
00043 #include <geometry_msgs/Point.h>
00044 #include <geometry_msgs/Quaternion.h>
00045 
00046 #include <bmg160.hpp>
00047 #include <sub20_interface.hpp>
00048 
00049 // ROS message
00050 #include <bmg160_driver/bmg160meas.h>
00051 
00052 using namespace bosch_drivers_common;
00053 
00054 int main( int argc, char **argv )
00055 {  
00056   //ROS initialization and publisher/subscriber setup
00057   ros::init( argc, argv, "BMG160_node" );
00058   ros::NodeHandle nh;
00059 
00060   std::string hw_id;
00061   int number_of_bmg160_sensors;
00062   int publish_rate_Hz;
00063 
00064  
00065   // Get parameters from .launch file or parameter server, or take defaults
00066   nh.param<int>( "/bmg160_node/number_of_bmg160_sensors", number_of_bmg160_sensors, 1 );
00067   nh.param<int>( "/bmg160_node/publish_rate_Hz", publish_rate_Hz, 50 );
00068   nh.param<std::string>( "/bmg160_node/hardware_id", hw_id, "0208" );
00069 
00070   Sub20Interface sub20( hw_id ); // (Each sub20 is identified by its serial number.)
00071   std::vector<BMG160*> BMG160_Chain;
00072 
00073   // Intantiate the array of BMG160s
00074   for( int i = 0; i < number_of_bmg160_sensors; i++ )
00075   { 
00076     // instantiate a sensor and pass in a pointer to it's hw interface:
00077     BMG160* gyro_sensor = new BMG160( &sub20 ); 
00078   
00079     // Adjust sensor settings:
00080     gyro_sensor->setProtocol( SPI );
00081     gyro_sensor->setFrequency( 125000 );
00082     gyro_sensor->setPin( i );
00083   
00084     //gyro_sensor->setRange( BMG160Parameters::RANGE_125 );
00085     //gyro_sensor->setBandwidth( BMG160Parameters::BW_32HZ );
00086     //gyro_sensor->setFilter( false );
00087     // Initialize each gyro_sensor
00088     if( gyro_sensor->initialize() == false )
00089     {
00090       ROS_ERROR( "gyro_sensor initialization %d failed.", i );
00091       return 1;
00092     }
00093   
00094     // Calibrate each bmg160 in a static position:
00095     gyro_sensor->SimpleCalibration();      
00096 
00097     // Add the sensor to the sensor chain
00098     BMG160_Chain.push_back( gyro_sensor );
00099   }
00100  
00101   // Set up ROS message and publisher
00102   bmg160_driver::bmg160meas msg;
00103   ros::Publisher bmg160_pub = nh.advertise<bmg160_driver::bmg160meas>( "BMG160_data", 20 );   
00104 
00105   //set loop rate for measurement polling
00106   ros::Rate loop_rate_Hz( publish_rate_Hz ); 
00107     
00108   // Run sensor node:
00109   while( nh.ok() )
00110   {
00111     // remove old data on all vectors, so data doesn't accumulate:
00112     msg.GyroX.clear();
00113     msg.GyroY.clear();
00114     msg.GyroZ.clear();
00115     msg.GyroTemperature.clear();  
00116 
00117     for( int j = 0; j < number_of_bmg160_sensors; j++ )
00118     {
00119       // Read data from IMU and pass on Chip Select:
00120 
00121       if( BMG160_Chain[j]->takeMeasurement() == false )
00122       {
00123         ROS_ERROR( "Failure: gyro_sensor device %d", j );
00124         return 1;
00125       }
00126  
00127       msg.GyroX.push_back( BMG160_Chain[j]->GyroX_ );
00128       msg.GyroY.push_back( BMG160_Chain[j]->GyroY_ );
00129       msg.GyroZ.push_back( BMG160_Chain[j]->GyroZ_ );
00130   
00131       msg.GyroTemperature.push_back( BMG160_Chain[j]->Temperature_ );
00132     }
00133     msg.header.stamp = ros::Time::now();
00134 
00135     bmg160_pub.publish( msg ); 
00136 
00137     ros::spinOnce();
00138     loop_rate_Hz.sleep();
00139   }
00140   
00141   ROS_WARN( "Closing BMG160 driver." );
00142   return 0;
00143 }


bosch_drivers_basic_nodes
Author(s): Joshua Vasquez, Philip Roan. Maintained by Philip Roan
autogenerated on Sat Dec 28 2013 16:49:50