48 this->
pub_header_ = this->
nh_.
advertise<std_msgs::Header>(
"/projector_controller/rising_edge_timestamps", 1,
true);
50 this->
srv_.setCallback(f);
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);
74 if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOff)
78 else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorAuto)
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)
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)
93 ROS_WARN_NAMED(
"vision_reconfigure",
"Alternate Projector Mode not supported in simulation, setting projector to on for now");
98 ROS_DEBUG_NAMED(
"vision_reconfigure",
"Projector only supported for modes: WithProjector and AlternateProjector");
102 else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOn)
108 ROS_ERROR_NAMED(
"vision_reconfigure",
"projector_mode is not in any recognized state [%d]",config.projector_mode);
117 static const double timeout = 0.01;
118 while (this->
nh_.
ok())
130 rh.frame_id =
"projector_wg6802418_frame";
138 while(this->
nh_.
ok())
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
#define ROS_DEBUG_NAMED(name,...)
void setCallbackQueue(CallbackQueueInterface *queue)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
ROSCPP_DECL void spinOnce()