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   this->pub_header_ = this->nh_.advertise<std_msgs::Header>("/projector_controller/rising_edge_timestamps", 1,true); // publish latched for sim
00049   dynamic_reconfigure::Server<gazebo_plugins::CameraSynchronizerConfig>::CallbackType f = boost::bind(&VisionReconfigure::ReconfigureCallback, this, _1, _2);
00050   this->srv_.setCallback(f);
00051 
00052 
00053   // initialize from relevant params on server
00054   gazebo_plugins::CameraSynchronizerConfig config;
00055   this->nh_.getParam("/camera_synchronizer_node/projector_mode",config.projector_mode);
00056   this->nh_.getParam("/camera_synchronizer_node/forearm_l_trig_mode",config.forearm_l_trig_mode);
00057   this->nh_.getParam("/camera_synchronizer_node/forearm_r_trig_mode",config.forearm_r_trig_mode);
00058   this->nh_.getParam("/camera_synchronizer_node/narrow_stereo_trig_mode",config.narrow_stereo_trig_mode);
00059   this->nh_.getParam("/camera_synchronizer_node/wide_stereo_trig_mode",config.wide_stereo_trig_mode);
00060   this->ReconfigureCallback(config,0);
00061 
00062 }
00063 
00064 VisionReconfigure::~VisionReconfigure()
00065 {
00066   this->nh_.shutdown();
00067   this->callback_queue_thread_.join();
00068 }
00069 
00070 void VisionReconfigure::ReconfigureCallback(gazebo_plugins::CameraSynchronizerConfig &config, uint32_t level)
00071 {
00072 
00073   // turn on or off projector in gazebo plugin
00074   if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOff)
00075   {
00076     this->projector_msg_.data = 0;
00077   }
00078   else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorAuto)
00079   {
00080     // turn on or off projector based on narrow stereo trigger mode
00081     if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
00082         config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
00083         config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
00084         config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector)
00085     {
00086       this->projector_msg_.data = 1;
00087     }
00088     else if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
00089              config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
00090              config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
00091              config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector)
00092     {
00093       ROS_WARN("Alternate Projector Mode not supported in simulation, setting projector to on for now");
00094       this->projector_msg_.data = 1;
00095     }
00096     else
00097     {
00098       ROS_DEBUG("Projector only supported for modes: WithProjector and AlternateProjector");
00099       this->projector_msg_.data = 0;
00100     }
00101   }
00102   else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOn)
00103   {
00104     this->projector_msg_.data = 1;
00105   }
00106   else
00107   {
00108     ROS_ERROR("projector_mode is not in any recognized state [%d]",config.projector_mode);
00109   }
00110 
00111   this->pub_projector_.publish(projector_msg_);
00112 }
00113 
00114 void VisionReconfigure::QueueThread()
00115 {
00116   // FIXME: hardcoded to 100Hz update rate for ros callback queue
00117   static const double timeout = 0.01;
00118   while (this->nh_.ok())
00119   {
00120     this->queue_.callAvailable(ros::WallDuration(timeout));
00121   }
00122 }
00123 
00124 void VisionReconfigure::spinOnce()
00125 {
00126   if (projector_msg_.data == 1)
00127   {
00128     std_msgs::Header rh;
00129     rh.stamp = ros::Time::now();
00130     rh.frame_id = "projector_wg6802418_frame";
00131     this->pub_header_.publish(rh);
00132   }
00133 }
00134 
00135 void VisionReconfigure::spin(double spin_frequency)
00136 {
00137   ros::Rate loop_rate(spin_frequency);
00138   while(this->nh_.ok())
00139   {
00140     ros::spinOnce();
00141     this->spinOnce();
00142     loop_rate.sleep();
00143   }
00144 }
00145 


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09