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 <geometry_msgs/Point.h>
00044 #include <geometry_msgs/Quaternion.h>
00045
00046 #include <bmg160.hpp>
00047 #include <sub20_interface.hpp>
00048
00049
00050 #include <bmg160_driver/bmg160meas.h>
00051
00052 using namespace bosch_drivers_common;
00053
00054 int main( int argc, char **argv )
00055 {
00056
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
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 );
00071 std::vector<BMG160*> BMG160_Chain;
00072
00073
00074 for( int i = 0; i < number_of_bmg160_sensors; i++ )
00075 {
00076
00077 BMG160* gyro_sensor = new BMG160( &sub20 );
00078
00079
00080 gyro_sensor->setProtocol( SPI );
00081 gyro_sensor->setFrequency( 125000 );
00082 gyro_sensor->setPin( i );
00083
00084
00085
00086
00087
00088 if( gyro_sensor->initialize() == false )
00089 {
00090 ROS_ERROR( "gyro_sensor initialization %d failed.", i );
00091 return 1;
00092 }
00093
00094
00095 gyro_sensor->SimpleCalibration();
00096
00097
00098 BMG160_Chain.push_back( gyro_sensor );
00099 }
00100
00101
00102 bmg160_driver::bmg160meas msg;
00103 ros::Publisher bmg160_pub = nh.advertise<bmg160_driver::bmg160meas>( "BMG160_data", 20 );
00104
00105
00106 ros::Rate loop_rate_Hz( publish_rate_Hz );
00107
00108
00109 while( nh.ok() )
00110 {
00111
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
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 }