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 <bmp085.hpp> 00044 #include <sub20_interface.hpp> 00045 00046 // ROS message 00047 #include <bmp085_driver/bmp085_measurement.h> 00048 00049 00058 int main( int argc, char **argv ) 00059 { 00060 //ROS initialization and publisher/subscriber setup 00061 ros::init( argc, argv, "BMP085_node" ); 00062 ros::NodeHandle nh; 00063 00064 std::string hw_id; 00065 int publish_rate_Hz; 00066 00067 00068 // Get parameters from .launch file or parameter server, or take defaults 00069 nh.param<int>( "/bmp085_node/publish_rate_Hz", publish_rate_Hz, 10 ); 00070 nh.param<std::string>( "/bmp085_node/hardware_id", hw_id, "0208" ); 00071 00072 Sub20Interface sub20( hw_id ); 00073 BMP085 PressureSensor( &sub20 ); 00074 00075 // Apply any sensor settings that deviate from the default settings: 00076 //PressureSensor.setSamplingMode( BMP085Parameters::ULTRA_HIGH_RESOLUTION ); 00077 00078 // Initialize the Pressure Sensor: 00079 if( PressureSensor.initialize() == false ) 00080 { 00081 ROS_ERROR( "BMP085 initialization failed." ); 00082 return 1; 00083 } 00084 00085 // Set up ROS message and publisher 00086 bmp085_driver::bmp085_measurement msg; 00087 ros::Publisher bmp085_pub = nh.advertise<bmp085_driver::bmp085_measurement>( "BMP085_data", 10 ); // buffer up to 10 messages of data. 00088 00089 //set loop rate for measurement polling 00090 ros::Rate loop_rate_Hz( publish_rate_Hz ); 00091 00092 // Run sensor node: 00093 while( nh.ok() ) 00094 { 00095 // stuff message: 00096 msg.header.stamp = ros::Time::now(); 00097 00098 // take measurement; halt node if false. 00099 if( PressureSensor.takeMeasurement() == false ) 00100 { 00101 ROS_ERROR( "BMP085 device failed to takeMeasurement()." ); 00102 return 1; 00103 } 00104 00105 msg.Temperature = PressureSensor.getTemperature(); 00106 msg.Pressure = PressureSensor.getPressure(); 00107 msg.Altitude = PressureSensor.getAltitude(); 00108 bmp085_pub.publish( msg ); 00109 00110 ros::spinOnce(); 00111 loop_rate_Hz.sleep(); 00112 } 00113 00114 ROS_WARN( "Closing BMP085 driver." ); 00115 return 0; 00116 }