vision_reconfigure.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009-2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 
38 {
39  this->nh_.setCallbackQueue(&this->queue_);
40 
41  // Custom Callback Queue
42  this->callback_queue_thread_ = boost::thread( boost::bind( &VisionReconfigure::QueueThread,this ) );
43 
44  // this code needs to be rewritten
45  // for now, it publishes on pub_projector_ which is used by gazebo_ros_projector plugin directly
46  // and it publishes pub_header_, which is published by projector_controller in ethercat_trigger_controllers package in real life
47  this->pub_projector_ = this->nh_.advertise<std_msgs::Int32>("/projector_wg6802418_controller/projector", 1,true); // publish latched for sim
48  this->pub_header_ = this->nh_.advertise<std_msgs::Header>("/projector_controller/rising_edge_timestamps", 1,true); // publish latched for sim
49  dynamic_reconfigure::Server<gazebo_plugins::CameraSynchronizerConfig>::CallbackType f = boost::bind(&VisionReconfigure::ReconfigureCallback, this, _1, _2);
50  this->srv_.setCallback(f);
51 
52 
53  // initialize from relevant params on server
54  gazebo_plugins::CameraSynchronizerConfig config;
55  this->nh_.getParam("/camera_synchronizer_node/projector_mode",config.projector_mode);
56  this->nh_.getParam("/camera_synchronizer_node/forearm_l_trig_mode",config.forearm_l_trig_mode);
57  this->nh_.getParam("/camera_synchronizer_node/forearm_r_trig_mode",config.forearm_r_trig_mode);
58  this->nh_.getParam("/camera_synchronizer_node/narrow_stereo_trig_mode",config.narrow_stereo_trig_mode);
59  this->nh_.getParam("/camera_synchronizer_node/wide_stereo_trig_mode",config.wide_stereo_trig_mode);
60  this->ReconfigureCallback(config,0);
61 
62 }
63 
65 {
66  this->nh_.shutdown();
67  this->callback_queue_thread_.join();
68 }
69 
70 void VisionReconfigure::ReconfigureCallback(gazebo_plugins::CameraSynchronizerConfig &config, uint32_t level)
71 {
72 
73  // turn on or off projector in gazebo plugin
74  if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOff)
75  {
76  this->projector_msg_.data = 0;
77  }
78  else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorAuto)
79  {
80  // turn on or off projector based on narrow stereo trigger mode
81  if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
82  config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
83  config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector ||
84  config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector)
85  {
86  this->projector_msg_.data = 1;
87  }
88  else if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
89  config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
90  config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector ||
91  config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector)
92  {
93  ROS_WARN_NAMED("vision_reconfigure", "Alternate Projector Mode not supported in simulation, setting projector to on for now");
94  this->projector_msg_.data = 1;
95  }
96  else
97  {
98  ROS_DEBUG_NAMED("vision_reconfigure", "Projector only supported for modes: WithProjector and AlternateProjector");
99  this->projector_msg_.data = 0;
100  }
101  }
102  else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOn)
103  {
104  this->projector_msg_.data = 1;
105  }
106  else
107  {
108  ROS_ERROR_NAMED("vision_reconfigure", "projector_mode is not in any recognized state [%d]",config.projector_mode);
109  }
110 
112 }
113 
115 {
116  // FIXME: hardcoded to 100Hz update rate for ros callback queue
117  static const double timeout = 0.01;
118  while (this->nh_.ok())
119  {
120  this->queue_.callAvailable(ros::WallDuration(timeout));
121  }
122 }
123 
125 {
126  if (projector_msg_.data == 1)
127  {
128  std_msgs::Header rh;
129  rh.stamp = ros::Time::now();
130  rh.frame_id = "projector_wg6802418_frame";
131  this->pub_header_.publish(rh);
132  }
133 }
134 
135 void VisionReconfigure::spin(double spin_frequency)
136 {
137  ros::Rate loop_rate(spin_frequency);
138  while(this->nh_.ok())
139  {
140  ros::spinOnce();
141  this->spinOnce();
142  loop_rate.sleep();
143  }
144 }
ros::CallbackQueue queue_
void spin(double spin_frequency)
ros::NodeHandle nh_
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
f
std_msgs::Int32 projector_msg_
boost::thread callback_queue_thread_
#define ROS_DEBUG_NAMED(name,...)
void setCallbackQueue(CallbackQueueInterface *queue)
ros::Publisher pub_header_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_projector_
bool sleep()
dynamic_reconfigure::Server< gazebo_plugins::CameraSynchronizerConfig > srv_
bool getParam(const std::string &key, std::string &s) const
void ReconfigureCallback(gazebo_plugins::CameraSynchronizerConfig &config, uint32_t level)
#define ROS_ERROR_NAMED(name,...)
static Time now()
bool ok() const
ROSCPP_DECL void spinOnce()


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27