roll_pitch_stabilizer.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 
00030 #include <ros/ros.h>
00031 #include <tf/transform_listener.h>
00032 #include <std_msgs/Float64.h>
00033 #include <string>
00034 #include "hector_roll_pitch_stabilizer/DoScan.h"
00035 
00036 std::string p_base_frame_;
00037 std::string p_base_stabilized_frame_;
00038 
00039 
00040 tf::TransformListener* tfL_;
00041 tf::StampedTransform transform_;
00042 
00043 ros::Publisher pub_desired_roll_angle_;
00044 ros::Publisher pub_desired_pitch_angle_;
00045 ros::ServiceServer scan_server_;
00046 
00047 bool updatesEnabled = true;
00048 
00049 void stabilize() {
00050   try
00051   {
00052       tfL_->lookupTransform(p_base_frame_, p_base_stabilized_frame_, ros::Time(0), transform_);
00053 
00054       tfScalar yaw, pitch, roll;
00055       transform_.getBasis().getEulerYPR(yaw, pitch, roll);
00056 
00057       std_msgs::Float64 tmp;
00058 
00059       tmp.data = -roll;
00060       pub_desired_roll_angle_.publish(tmp);
00061 
00062       tmp.data = -pitch;
00063       pub_desired_pitch_angle_.publish(tmp);    
00064   }
00065   catch(tf::TransformException e)
00066   {
00067     //ROS_ERROR("Transform failed %s",e.what());
00068     //odom_to_map.setIdentity();
00069   }
00070 }
00071 
00072 void updateTimerCallback(const ros::TimerEvent& event)
00073 {
00074   if ( updatesEnabled ) {
00075     stabilize();
00076   }
00077 }
00078 
00079 bool doScan(hector_roll_pitch_stabilizer::DoScan::Request &request, hector_roll_pitch_stabilizer::DoScan::Response &response) {
00080   updatesEnabled = false;
00081   
00082   std_msgs::Float64 tmp;
00083   
00084   //request.max_angle_pitch, request.step, request.sleep_time_ms);
00085 
00086   stabilize();
00087     
00088   // traverse pitch angle
00089   for ( double pos = 0; pos > request.min_angle_pitch; pos -= request.step ) {
00090     tmp.data = pos;
00091     pub_desired_pitch_angle_.publish(tmp);
00092     usleep(1000*request.sleep_time_ms);
00093   }
00094   
00095   for ( double pos = request.min_angle_pitch; pos <= request.max_angle_pitch; pos += request.step ) {
00096     tmp.data = pos;
00097     pub_desired_pitch_angle_.publish(tmp);
00098     usleep(1000*request.sleep_time_ms);
00099   }
00100  
00101   for ( double pos = request.max_angle_pitch; pos >= 0.0; pos -= request.step ) {
00102     tmp.data = pos;
00103     pub_desired_pitch_angle_.publish(tmp);
00104     usleep(1000*request.sleep_time_ms);
00105   }
00106   
00107   // traverse roll angle
00108   for ( double pos = 0; pos > request.min_angle_roll; pos -= request.step ) {
00109     tmp.data = pos;
00110     pub_desired_pitch_angle_.publish(tmp);
00111     usleep(1000*request.sleep_time_ms);
00112   }
00113   
00114   for ( double pos = request.min_angle_roll; pos <= request.max_angle_roll; pos += request.step ) {
00115     tmp.data = pos;
00116     pub_desired_roll_angle_.publish(tmp);
00117     usleep(1000*request.sleep_time_ms);
00118   }
00119     
00120   for ( double pos = request.max_angle_roll; pos >= 0.0; pos -= request.step ) {
00121     tmp.data = pos;
00122     pub_desired_pitch_angle_.publish(tmp);
00123     usleep(1000*request.sleep_time_ms);
00124   }
00125   
00126   updatesEnabled = true;  
00127   
00128   return true;
00129 }
00130 
00131 int main(int argc, char **argv) {
00132   ros::init(argc, argv, ROS_PACKAGE_NAME);
00133   ros::NodeHandle n;
00134   ros::NodeHandle pn("~");
00135 
00136   pn.param("base_frame", p_base_frame_, std::string("base_frame"));
00137   pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized_frame"));
00138 
00139   tfL_ = new tf::TransformListener();
00140 
00141   ros::Timer update_timer = pn.createTimer(ros::Duration(1.0 / 30), &updateTimerCallback, false);
00142 
00143   pub_desired_roll_angle_ = pn.advertise<std_msgs::Float64>("/desired_roll_angle",10,false);
00144   pub_desired_pitch_angle_ = pn.advertise<std_msgs::Float64>("/desired_pitch_angle",10,false);
00145   
00146   scan_server_ = n.advertiseService(std::string("/hector_roll_pitch_stabilizer/do_scan"), &doScan);
00147 
00148   ros::spin();
00149 
00150   delete tfL_;
00151 
00152   return 0;
00153 }
00154 


hector_roll_pitch_stabilizer
Author(s): Stefan Kohlbrecher
autogenerated on Wed Sep 16 2015 04:29:57