Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <cmath>
00040
00041
00042 #include <ros/ros.h>
00043 #include <ros/time.h>
00044
00045 #include <bma180.hpp>
00046 #include <sub20_interface.hpp>
00047
00048
00049 #include <bma180_driver/bma180_measurement.h>
00050
00059 int main( int argc, char **argv )
00060 {
00061
00062 ros::init(argc, argv, "BMA180_node");
00063 ros::NodeHandle nh;
00064
00065
00066
00071 std::string hw_id;
00078 int number_of_bma180_sensors;
00079 int publish_rate_Hz;
00080
00081
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
00091 for( int i = 0; i < number_of_bma180_sensors; i++ )
00092 {
00093
00094 BMA180* Accelerometer = new BMA180( &sub20 );
00095
00096
00097 Accelerometer->setAccelRange( BMA180Parameters::RANGE_8 );
00098 Accelerometer->setBandwidth( BMA180Parameters::BW_150 );
00099
00100
00101 Accelerometer->setProtocol( SPI );
00102 Accelerometer->setFrequency( 125000 );
00103 Accelerometer->setPin( i );
00104
00105
00106
00107
00108
00109
00110 if( Accelerometer->initialize() == false )
00111 {
00112 ROS_ERROR( "BMA180 device: %d initialization failed.", i );
00113
00114 }
00115
00116
00117 bma180_chain.push_back( Accelerometer );
00118 }
00119
00120
00121 bma180_driver::bma180_measurement msg;
00122 ros::Publisher bma180_pub = nh.advertise<bma180_driver::bma180_measurement>( "BMA180_data", 20 );
00123
00124
00125 ros::Rate loop_rate_Hz( publish_rate_Hz );
00126
00127 double pitch, roll;
00128
00129
00130 while( nh.ok() )
00131 {
00132
00133 for( int j = 0; j < number_of_bma180_sensors; j++ )
00134 {
00135
00136 if( bma180_chain[j]->takeMeasurement() == false )
00137 {
00138 ROS_ERROR( "BMA180 device %d failed to acquire measurements.", j );
00139
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
00156 msg.header.stamp = ros::Time::now();
00157 bma180_pub.publish( msg );
00158
00159
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 }