vision_reconfigure.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009-2010, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <gazebo_plugins/vision_reconfigure.h>
00036 
00037 VisionReconfigure::VisionReconfigure() : nh_("")
00038 {
00039   this->nh_.setCallbackQueue(&this->queue_);
00040 
00041   // Custom Callback Queue
00042   this->callback_queue_thread_ = boost::thread( boost::bind( &VisionReconfigure::QueueThread,this ) );
00043 
00044   // this code needs to be rewritten
00045   // for now, it publishes on pub_projector_ which is used by gazebo_ros_projector plugin directly
00046   //          and it publishes pub_header_, which is published by projector_controller in ethercat_trigger_controllers package in real life
00047   this->pub_projector_ = this->nh_.advertise<std_msgs::Int32>("/projector_wg6802418_controller/projector", 1,true); // publish latched for sim
00048 #if ROS_VERSION_MINIMUM(1, 3, 1)
00049   this->pub_header_ = this->nh_.advertise<std_msgs::Header>("/projector_controller/rising_edge_timestamps", 1,true); // publish latched for sim
00050 #else
00051   this->pub_header_ = this->nh_.advertise<roslib::Header>("/projector_controller/rising_edge_timestamps", 1,true); // publish latched for sim
00052 #endif
00053   dynamic_reconfigure::Server<gazebo_plugins::CameraSynchronizerConfig>::CallbackType f = boost::bind(&VisionReconfigure::ReconfigureCallback, this, _1, _2);
00054   this->srv_.setCallback(f);
00055 
00056 
00057   // initialize from relevant params on server
00058   gazebo_plugins::CameraSynchronizerConfig config;
00059   this->nh_.getParam("/camera_synchronizer_node/projector_mode",config.projector_mode);
00060   this->nh_.getParam("/camera_synchronizer_node/forearm_l_trig_mode",config.forearm_l_trig_mode);
00061   this->nh_.getParam("/camera_synchronizer_node/forearm_r_trig_mode",config.forearm_r_trig_mode);
00062   this->nh_.getParam("/camera_synchronizer_node/narrow_stereo_trig_mode",config.narrow_stereo_trig_mode);
00063   this->nh_.getParam("/camera_synchronizer_node/wide_stereo_trig_mode",config.wide_stereo_trig_mode);
00064   this->ReconfigureCallback(config,0);
00065 
00066 }
00067 
00068 VisionReconfigure::~VisionReconfigure()
00069 {
00070   this->nh_.shutdown();
00071   this->callback_queue_thread_.join();
00072 }
00073 
00074 void VisionReconfigure::ReconfigureCallback(gazebo_plugins::CameraSynchronizerConfig &config, uint32_t level)
00075 {
00076 
00077   // turn on or off projector in gazebo plugin
00078   if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOff)
00079   {
00080     this->projector_msg_.data = 0;
00081   }
00082   else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorAuto)
00083   {
00084     // turn on or off projector based on narrow stereo trigger mode
00085     if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
00086         config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
00087         config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
00088         config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector)
00089     {
00090       this->projector_msg_.data = 1;
00091     }
00092     else if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
00093              config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
00094              config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
00095              config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector)
00096     {
00097       ROS_WARN("Alternate Projector Mode not supported in simulation, setting projector to on for now");
00098       this->projector_msg_.data = 1;
00099     }
00100     else
00101     {
00102       ROS_DEBUG("Projector only supported for modes: WithProjector and AlternateProjector");
00103       this->projector_msg_.data = 0;
00104     }
00105   }
00106   else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOn)
00107   {
00108     this->projector_msg_.data = 1;
00109   }
00110   else
00111   {
00112     ROS_ERROR("projector_mode is not in any recognized state [%d]",config.projector_mode);
00113   }
00114 
00115   this->pub_projector_.publish(projector_msg_);
00116 }
00117 
00118 void VisionReconfigure::QueueThread()
00119 {
00120   // FIXME: hardcoded to 100Hz update rate for ros callback queue
00121   static const double timeout = 0.01;
00122   while (this->nh_.ok())
00123   {
00124     this->queue_.callAvailable(ros::WallDuration(timeout));
00125   }
00126 }
00127 
00128 void VisionReconfigure::spinOnce()
00129 {
00130   if (projector_msg_.data == 1)
00131   {
00132 #if ROS_VERSION_MINIMUM(1, 3, 1)
00133     std_msgs::Header rh;
00134 #else
00135     roslib::Header rh;
00136 #endif
00137     rh.stamp = ros::Time::now();
00138     rh.frame_id = "projector_wg6802418_frame";
00139     this->pub_header_.publish(rh);
00140   }
00141 }
00142 
00143 void VisionReconfigure::spin(double spin_frequency)
00144 {
00145   ros::Rate loop_rate(spin_frequency);
00146   while(this->nh_.ok())
00147   {
00148     ros::spinOnce();
00149     this->spinOnce();
00150     loop_rate.sleep();
00151   }
00152 }
00153 


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Sun Jan 5 2014 11:34:58