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
00040 #include <ros/ros.h>
00041 #include <ros/time.h>
00042
00043 #include <bmc050.hpp>
00044 #include <sub20_interface.hpp>
00045
00046
00047 #include <bmc050_driver/bmc050_measurement.h>
00048
00049 int main( int argc, char **argv )
00050 {
00051
00052
00053 ros::init( argc, argv, "bmc050_node" );
00054 ros::NodeHandle nh;
00055
00056 std::string hw_id;
00057 int number_of_bmc050_sensors;
00058 int publish_rate_Hz;
00059
00060
00061 nh.param<int>( "/bmc050_node/number_of_bmc050_sensors", number_of_bmc050_sensors, 1 );
00062 nh.param<int>( "/bmc050_node/publish_rate_Hz", publish_rate_Hz, 50 );
00063 nh.param<std::string>( "/bmc050_node/hardware_id", hw_id, "0208" );
00064
00065 Sub20Interface sub20( hw_id );
00066 std::vector<BMC050*> BMC050_Chain;
00067
00068
00069 for( int i = 0; i < number_of_bmc050_sensors; i++ )
00070 {
00071
00072 BMC050* eCompass = new BMC050( &sub20 );
00073
00074 eCompass->setProtocol( SPI );
00075 eCompass->setFrequency( 125000 );
00076 eCompass->setCompassPin( i*2 );
00077 eCompass->setAccelPin( i*2 + 1 );
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090 if( eCompass->initialize() == false )
00091 {
00092 ROS_ERROR( "BMC050 device: %d initialization failed.", i );
00093 return 1;
00094 }
00095
00096
00097
00098 BMC050_Chain.push_back( eCompass );
00099 }
00100
00101
00102 bmc050_driver::bmc050_measurement msg;
00103 ros::Publisher bmc050_pub = nh.advertise<bmc050_driver::bmc050_measurement>("BMC050_data", 20 );
00104
00105
00106 ros::Rate loop_rate_Hz( publish_rate_Hz );
00107
00108
00109 while( nh.ok() )
00110 {
00111
00112 msg.AccelerationX.clear();
00113 msg.AccelerationY.clear();
00114 msg.AccelerationZ.clear();
00115 msg.Temperature.clear();
00116 msg.MagneticFieldIntensityX.clear();
00117 msg.MagneticFieldIntensityY.clear();
00118 msg.MagneticFieldIntensityZ.clear();
00119 msg.HallResistance.clear();
00120
00121 for( int i = 0; i < number_of_bmc050_sensors; i++ )
00122 {
00123
00124 if( BMC050_Chain[i]->takeMeasurement() == false )
00125 {
00126 ROS_ERROR( "BMC050 device %d failed. Node failed.", i );
00127 return 1;
00128 }
00129
00130 msg.AccelerationX.push_back( BMC050_Chain[i]->AccelX_ );
00131 msg.AccelerationY.push_back( BMC050_Chain[i]->AccelY_ );
00132 msg.AccelerationZ.push_back( BMC050_Chain[i]->AccelZ_ );
00133 msg.Temperature.push_back( BMC050_Chain[i]->Temperature_ );
00134 msg.MagneticFieldIntensityX.push_back( BMC050_Chain[i]->CompassX_ );
00135 msg.MagneticFieldIntensityY.push_back( BMC050_Chain[i]->CompassY_ );
00136 msg.MagneticFieldIntensityZ.push_back( BMC050_Chain[i]->CompassZ_ );
00137 msg.HallResistance.push_back( BMC050_Chain[i]->RHall_ );
00138 }
00139 msg.header.stamp = ros::Time::now();
00140
00141 bmc050_pub.publish( msg );
00142
00143 ros::spinOnce();
00144 loop_rate_Hz.sleep();
00145 }
00146
00147 ROS_WARN( "Closing BMC050 driver." );
00148 return 0;
00149 }