bma180_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 #include <cmath> // for M_PI
00040 
00041 // ROS headers
00042 #include <ros/ros.h>
00043 #include <ros/time.h>
00044 
00045 #include <bma180.hpp>
00046 #include <sub20_interface.hpp> 
00047 
00048 // ROS message
00049 #include <bma180_driver/bma180_measurement.h>
00050 
00059 int main( int argc, char **argv )
00060 {
00061   //ROS initialization and publisher/subscriber setup
00062   ros::init(argc, argv, "BMA180_node");
00063   ros::NodeHandle nh;
00064 
00065   // User configurable parameters
00066 
00071   std::string hw_id;
00078   int number_of_bma180_sensors;
00079   int publish_rate_Hz;
00080 
00081   // Get parameters from .launch file or parameter server, or take defaults
00082   nh.param<int>( "/bma180_node/number_of_bma180_sensors", number_of_bma180_sensors, 1 );
00083   nh.param<int>( "/bma180_node/publish_rate_Hz", publish_rate_Hz, 100 );
00084   nh.param<std::string>( "/bma180_node/hardware_id", hw_id, "0208" );
00085 
00086 
00087   Sub20Interface sub20( hw_id );   
00088   std::vector<BMA180*> bma180_chain;
00089 
00090   // Initialize the BMA180s
00091   for( int i = 0; i < number_of_bma180_sensors; i++ )
00092   {
00093     // instantiate a sensor and pass in a pointer to its hardware interface
00094     BMA180* Accelerometer = new BMA180( &sub20 ); 
00095 
00096     // Adjust sensor settings:
00097     Accelerometer->setAccelRange( BMA180Parameters::RANGE_8 );
00098     Accelerometer->setBandwidth( BMA180Parameters::BW_150 );
00099 
00100     // SPI configuration
00101     Accelerometer->setProtocol( SPI );
00102     Accelerometer->setFrequency( 125000 );
00103     Accelerometer->setPin( i );
00104 
00105     // I2C configuration
00106     //Accelerometer->setProtocol(I2C); // CSB connected to VDD
00107     //Accelerometer->setFrequency( 400000 );
00108     //Accelerometer->setSlaveAddressBit( 0 );
00109 
00110     if( Accelerometer->initialize() == false )
00111     {
00112       ROS_ERROR( "BMA180 device: %d initialization failed.", i );
00113       //return 0;
00114     }
00115 
00116      // Add the sensor to the sensor chain
00117     bma180_chain.push_back( Accelerometer );
00118   }
00119   
00120   // Set up ROS message and publisher
00121   bma180_driver::bma180_measurement msg;
00122   ros::Publisher bma180_pub = nh.advertise<bma180_driver::bma180_measurement>( "BMA180_data", 20 );
00123 
00124   //set loop rate for measurement polling
00125   ros::Rate loop_rate_Hz( publish_rate_Hz ); 
00126     
00127   double pitch, roll;
00128   
00129   // Run sensor node
00130   while( nh.ok() )
00131   {
00132     // Data goes in a standard vector
00133     for( int j = 0; j < number_of_bma180_sensors; j++ )
00134     {
00135       // Read measurements from sensors
00136       if( bma180_chain[j]->takeMeasurement() == false )
00137       {
00138         ROS_ERROR( "BMA180 device %d failed to acquire measurements.", j );
00139         //return 0;
00140       }
00141 
00142       roll = bma180_chain[j]->getStaticRoll() * (180.0/M_PI);   
00143       pitch = bma180_chain[j]->getStaticPitch() * (180.0/M_PI);
00144   
00145       msg.AccelerationX.push_back( bma180_chain[j]->AccelX_ );
00146       msg.AccelerationY.push_back( bma180_chain[j]->AccelY_ );
00147       msg.AccelerationZ.push_back( bma180_chain[j]->AccelZ_ );
00148    
00149       msg.Temperature.push_back( bma180_chain[j]->Temperature_ );
00150    
00151       msg.staticRoll.push_back( roll );
00152       msg.staticPitch.push_back( pitch );
00153     }
00154   
00155     // publish message
00156     msg.header.stamp = ros::Time::now();
00157     bma180_pub.publish( msg ); 
00158 
00159     // clear vectors so data doesn't accumulate
00160     msg.AccelerationX.clear();
00161     msg.AccelerationY.clear();
00162     msg.AccelerationZ.clear();
00163     msg.Temperature.clear();
00164     msg.staticRoll.clear();
00165     msg.staticPitch.clear();
00166     
00167     ros::spinOnce();
00168     loop_rate_Hz.sleep();
00169   }
00170 
00171   ROS_WARN( "Closing BMA180 driver." );
00172   return 0;
00173 }


bosch_drivers_basic_nodes
Author(s): Joshua Vasquez, Philip Roan. Maintained by Philip Roan
autogenerated on Mon Oct 6 2014 10:10:31