r200_nodelet.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  Copyright (c) 2017, Intel Corporation
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 
00008  1. Redistributions of source code must retain the above copyright notice, this
00009  list of conditions and the following disclaimer.
00010 
00011  2. Redistributions in binary form must reproduce the above copyright notice,
00012  this list of conditions and the following disclaimer in the documentation
00013  and/or other materials provided with the distribution.
00014 
00015  3. Neither the name of the copyright holder nor the names of its contributors
00016  may be used to endorse or promote products derived from this software without
00017  specific prior written permission.
00018 
00019  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00023  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00024  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00025  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00027  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00028  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *******************************************************************************/
00030 
00031 #include <string>
00032 #include <vector>
00033 #include <cstdlib>
00034 #include <bitset>
00035 
00036 #include <realsense_camera/r200_nodelet.h>
00037 
00038 PLUGINLIB_EXPORT_CLASS(realsense_camera::R200Nodelet, nodelet::Nodelet)
00039 
00040 namespace realsense_camera
00041 {
00042   /*
00043    * Initialize the nodelet.
00044    */
00045   void R200Nodelet::onInit()
00046   {
00047     format_[RS_STREAM_COLOR] = RS_FORMAT_RGB8;
00048     encoding_[RS_STREAM_COLOR] = sensor_msgs::image_encodings::RGB8;
00049     cv_type_[RS_STREAM_COLOR] = CV_8UC3;
00050     unit_step_size_[RS_STREAM_COLOR] = sizeof(unsigned char) * 3;
00051 
00052     format_[RS_STREAM_DEPTH] = RS_FORMAT_Z16;
00053     encoding_[RS_STREAM_DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1;
00054     cv_type_[RS_STREAM_DEPTH] = CV_16UC1;
00055     unit_step_size_[RS_STREAM_DEPTH] = sizeof(uint16_t);
00056 
00057     format_[RS_STREAM_INFRARED] = RS_FORMAT_Y8;
00058     encoding_[RS_STREAM_INFRARED] = sensor_msgs::image_encodings::TYPE_8UC1;
00059     cv_type_[RS_STREAM_INFRARED] = CV_8UC1;
00060     unit_step_size_[RS_STREAM_INFRARED] = sizeof(unsigned char);
00061 
00062     format_[RS_STREAM_INFRARED2] = RS_FORMAT_Y8;
00063     encoding_[RS_STREAM_INFRARED2] = sensor_msgs::image_encodings::TYPE_8UC1;
00064     cv_type_[RS_STREAM_INFRARED2] = CV_8UC1;
00065     unit_step_size_[RS_STREAM_INFRARED2] = sizeof(unsigned char);
00066 
00067     max_z_ = R200_MAX_Z;
00068 
00069     SyncNodelet::onInit();
00070   }
00071 
00072   /*
00073    * Get the nodelet parameters.
00074    */
00075   void R200Nodelet::getParameters()
00076   {
00077     BaseNodelet::getParameters();
00078     pnh_.param("ir2_frame_id", frame_id_[RS_STREAM_INFRARED2], DEFAULT_IR2_FRAME_ID);
00079     pnh_.param("ir2_optical_frame_id", optical_frame_id_[RS_STREAM_INFRARED2], DEFAULT_IR2_OPTICAL_FRAME_ID);
00080     pnh_.param("enable_ir2", enable_[RS_STREAM_INFRARED2], ENABLE_IR2);
00081 
00082     // set IR2 stream to match depth
00083     width_[RS_STREAM_INFRARED2] = width_[RS_STREAM_DEPTH];
00084     height_[RS_STREAM_INFRARED2] = height_[RS_STREAM_DEPTH];
00085     fps_[RS_STREAM_INFRARED2] = fps_[RS_STREAM_DEPTH];
00086   }
00087 
00088   /*
00089    * Advertise topics.
00090    */
00091   void R200Nodelet::advertiseTopics()
00092   {
00093     BaseNodelet::advertiseTopics();
00094     ros::NodeHandle ir2_nh(nh_, IR2_NAMESPACE);
00095     image_transport::ImageTransport ir2_image_transport(ir2_nh);
00096     camera_publisher_[RS_STREAM_INFRARED2] = ir2_image_transport.advertiseCamera(IR2_TOPIC, 1);
00097   }
00098 
00099   /*
00100    * Set Dynamic Reconfigure Server and return the dynamic params.
00101    */
00102   std::vector<std::string> R200Nodelet::setDynamicReconfServer()
00103   {
00104     dynamic_reconf_server_.reset(new dynamic_reconfigure::Server<realsense_camera::r200_paramsConfig>(pnh_));
00105 
00106     // Get dynamic options from the dynamic reconfigure server.
00107     realsense_camera::r200_paramsConfig params_config;
00108     dynamic_reconf_server_->getConfigDefault(params_config);
00109     std::vector<realsense_camera::r200_paramsConfig::AbstractParamDescriptionConstPtr> param_desc =
00110         params_config.__getParamDescriptions__();
00111     std::vector<std::string> dynamic_params;
00112     for (realsense_camera::r200_paramsConfig::AbstractParamDescriptionConstPtr param_desc_ptr : param_desc)
00113     {
00114       dynamic_params.push_back((* param_desc_ptr).name);
00115     }
00116 
00117     return dynamic_params;
00118   }
00119 
00120   /*
00121    * Start Dynamic Reconfigure Callback.
00122    */
00123   void R200Nodelet::startDynamicReconfCallback()
00124   {
00125     dynamic_reconf_server_->setCallback(boost::bind(&R200Nodelet::configCallback, this, _1, _2));
00126   }
00127 
00128   /*
00129    * Change the Depth Control Preset
00130    */
00131   void R200Nodelet::setDynamicReconfigDepthControlPreset(int preset)
00132   {
00133     // There is not a C++ API for dynamic reconfig so we need to use a system call
00134     // Adding the sleep to ensure the current callback can end before we
00135     // attempt the next callback from the system call.
00136     std::vector<std::string> argv;
00137     argv.push_back("rosrun");
00138     argv.push_back("dynamic_reconfigure");
00139     argv.push_back("dynparam");
00140     argv.push_back("set");
00141     argv.push_back(nodelet_name_);
00142     argv.push_back("r200_dc_preset");
00143     argv.push_back(std::to_string(preset));
00144 
00145     wrappedSystem(argv);
00146   }
00147 
00148   /*
00149    * Change the Depth Control Individual values
00150    * GET ALL of the DC options from librealsense
00151    * Call dynamic reconfig and set all 10 values as a set
00152    */
00153   std::string R200Nodelet::setDynamicReconfigDepthControlIndividuals()
00154   {
00155     std::string current_param;
00156     std::string current_dc;
00157     std::string option_value;
00158 
00159     // There is not a C++ API for dynamic reconfig so we need to use a system call
00160     // Adding the sleep to ensure the current callback can end before we
00161     // attempt the next callback from the system call.
00162     std::vector<std::string> argv;
00163     argv.push_back("rosrun");
00164     argv.push_back("dynamic_reconfigure");
00165     argv.push_back("dynparam");
00166     argv.push_back("set");
00167     argv.push_back(nodelet_name_);
00168 
00169     current_param = "{";
00170 
00171     option_value =
00172       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00173               RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT, 0)));
00174     current_param += "'r200_dc_estimate_median_decrement':" + option_value + ", ";
00175     current_dc += option_value + ":";
00176 
00177     option_value =
00178       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00179               RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT, 0)));
00180     current_param += "'r200_dc_estimate_median_increment':" + option_value + ", ";
00181     current_dc += option_value + ":";
00182 
00183     option_value =
00184       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00185               RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD, 0)));
00186     current_param += "'r200_dc_median_threshold':" + option_value + ", ";
00187     current_dc += option_value + ":";
00188 
00189     option_value =
00190       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00191               RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD, 0)));
00192     current_param += "'r200_dc_score_minimum_threshold':" + option_value + ", ";
00193     current_dc += option_value + ":";
00194 
00195     option_value =
00196       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00197               RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD, 0)));
00198     current_param += "'r200_dc_score_maximum_threshold':" + option_value + ", ";
00199     current_dc += option_value + ":";
00200 
00201     option_value =
00202       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00203               RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD, 0)));
00204     current_param += "'r200_dc_texture_count_threshold':" + option_value + ", ";
00205     current_dc += option_value + ":";
00206 
00207     option_value =
00208       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00209               RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD, 0)));
00210     current_param += "'r200_dc_texture_difference_threshold':" + option_value + ", ";
00211     current_dc += option_value + ":";
00212 
00213     option_value =
00214       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00215               RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD, 0)));
00216     current_param += "'r200_dc_second_peak_threshold':" + option_value + ", ";
00217     current_dc += option_value + ":";
00218 
00219     option_value =
00220       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00221               RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD, 0)));
00222     current_param += "'r200_dc_neighbor_threshold':" + option_value + ", ";
00223     current_dc += option_value + ":";
00224 
00225     option_value =
00226       std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
00227               RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD, 0)));
00228     current_param += "'r200_dc_lr_threshold':" + option_value + "}";
00229     current_dc += option_value;
00230 
00231     ROS_DEBUG_STREAM(nodelet_name_ << " - Setting DC: " << current_param);
00232 
00233     argv.push_back(current_param);
00234 
00235     wrappedSystem(argv);
00236 
00237     return current_dc;
00238   }
00239 
00240   /*
00241    * Get the dynamic param values.
00242    */
00243   void R200Nodelet::configCallback(realsense_camera::r200_paramsConfig &config, uint32_t level)
00244   {
00245     // Save the dc_preset value as there is no getter API for this value
00246     static int dc_preset = -2;
00247     int previous_dc_preset = dc_preset;
00248     // Save the last depth control preset values as a string
00249     static std::string last_dc;
00250 
00251     // level is the ORing of all levels which have a changed value
00252     std::bitset<32> bit_level{level};
00253 
00254     if (bit_level.test(6))  // 2^6 = 64 : Depth Control Preset
00255     {
00256       ROS_INFO_STREAM(nodelet_name_ << " - Setting dynamic camera options" <<
00257           " (r200_dc_preset=" << config.r200_dc_preset << ")");
00258     }
00259     else
00260     {
00261       ROS_INFO_STREAM(nodelet_name_ << " - Setting dynamic camera options");
00262     }
00263 
00264     // set the depth enable
00265     BaseNodelet::setDepthEnable(config.enable_depth);
00266 
00267     // Set common options
00268     rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.color_backlight_compensation, 0);
00269     rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.color_brightness, 0);
00270     rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.color_contrast, 0);
00271     rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.color_gain, 0);
00272     rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.color_gamma, 0);
00273     rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.color_hue, 0);
00274     rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.color_saturation, 0);
00275     rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.color_sharpness, 0);
00276     rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_EXPOSURE,
00277     config.color_enable_auto_exposure, 0);
00278     if (config.color_enable_auto_exposure == 0)
00279     {
00280       rs_set_device_option(rs_device_, RS_OPTION_COLOR_EXPOSURE, config.color_exposure, 0);
00281     }
00282     rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE,
00283         config.color_enable_auto_white_balance, 0);
00284     if (config.color_enable_auto_white_balance == 0)
00285     {
00286       rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.color_white_balance, 0);
00287     }
00288 
00289     // Set R200 specific options
00290     rs_set_device_option(rs_device_, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, config.r200_lr_auto_exposure_enabled, 0);
00291     if (config.r200_lr_auto_exposure_enabled == 0)
00292     {
00293       rs_set_device_option(rs_device_, RS_OPTION_R200_LR_GAIN, config.r200_lr_gain, 0);
00294       rs_set_device_option(rs_device_, RS_OPTION_R200_LR_EXPOSURE, config.r200_lr_exposure, 0);
00295     }
00296 
00297     if (config.r200_lr_auto_exposure_enabled == 1)
00298     {
00299       if (config.r200_auto_exposure_top_edge >= height_[RS_STREAM_DEPTH])
00300       {
00301         config.r200_auto_exposure_top_edge = height_[RS_STREAM_DEPTH] - 1;
00302       }
00303       if (config.r200_auto_exposure_bottom_edge >= height_[RS_STREAM_DEPTH])
00304       {
00305         config.r200_auto_exposure_bottom_edge = height_[RS_STREAM_DEPTH] - 1;
00306       }
00307       if (config.r200_auto_exposure_left_edge >= width_[RS_STREAM_DEPTH])
00308       {
00309         config.r200_auto_exposure_left_edge = width_[RS_STREAM_DEPTH] - 1;
00310       }
00311       if (config.r200_auto_exposure_right_edge >= width_[RS_STREAM_DEPTH])
00312       {
00313         config.r200_auto_exposure_right_edge = width_[RS_STREAM_DEPTH] - 1;
00314       }
00315       double edge_values_[4];
00316       edge_values_[0] = config.r200_auto_exposure_left_edge;
00317       edge_values_[1] = config.r200_auto_exposure_top_edge;
00318       edge_values_[2] = config.r200_auto_exposure_right_edge;
00319       edge_values_[3] = config.r200_auto_exposure_bottom_edge;
00320       rs_set_device_options(rs_device_, edge_options_, 4, edge_values_, 0);
00321     }
00322 
00323     rs_set_device_option(rs_device_, RS_OPTION_R200_EMITTER_ENABLED, config.r200_emitter_enabled, 0);
00324 
00325     // Depth Control Group Settings
00326     // NOTE: do NOT use the config.groups values as they are zero the first time called
00327     if (bit_level.test(5))  // 2^5 = 32 : Individual Depth Control settings
00328     {
00329       std::string current_dc;
00330 
00331       ROS_DEBUG_STREAM(nodelet_name_ << " - Setting Individual Depth Control");
00332 
00333       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT,
00334           config.r200_dc_estimate_median_decrement, 0);
00335       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_estimate_median_decrement)) + ":";
00336       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT,
00337           config.r200_dc_estimate_median_increment, 0);
00338       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_estimate_median_increment)) + ":";
00339       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD,
00340           config.r200_dc_median_threshold, 0);
00341       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_median_threshold)) + ":";
00342       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD,
00343           config.r200_dc_score_minimum_threshold, 0);
00344       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_score_minimum_threshold)) + ":";
00345       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD,
00346           config.r200_dc_score_maximum_threshold, 0);
00347       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_score_maximum_threshold)) + ":";
00348       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD,
00349           config.r200_dc_texture_count_threshold, 0);
00350       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_texture_count_threshold)) + ":";
00351       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD,
00352           config.r200_dc_texture_difference_threshold, 0);
00353       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_texture_difference_threshold)) + ":";
00354       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD,
00355           config.r200_dc_second_peak_threshold, 0);
00356       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_second_peak_threshold)) + ":";
00357       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD,
00358           config.r200_dc_neighbor_threshold, 0);
00359       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_neighbor_threshold)) + ":";
00360       rs_set_device_option(rs_device_, RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD,
00361           config.r200_dc_lr_threshold, 0);
00362       current_dc += std::to_string(static_cast<uint32_t>(config.r200_dc_lr_threshold));
00363 
00364       // Preset also changed in the same update callback
00365       // This is either First callback special case, or both set via
00366       // dynamic configure command line.
00367       if (bit_level.test(6))  // 2^6 = 64 : Depth Control Preset
00368       {
00369         dc_preset = config.r200_dc_preset;
00370 
00371         if (previous_dc_preset != -2)  // not the first pass special case (-2)
00372         {
00373           // Changing individual Depth Control params means preset is Unused/Invalid
00374           // if the individual values are not the same as the preset values
00375           if (dc_preset != -1 && current_dc != last_dc)
00376           {
00377             ROS_DEBUG_STREAM(nodelet_name_ << " - Forcing Depth Control Preset to Unused");
00378             setDynamicReconfigDepthControlPreset(-1);
00379           }
00380         }
00381         else
00382         {
00383           // This is the first pass callback, in this instance we allow the
00384           // dc_preset to trump the individual values as it might have been
00385           // set from the launch file. To allow override of individual values,
00386           // set dc_preset to -1 (Unused) in the launch file.
00387           if (dc_preset != -1)
00388           {
00389             ROS_INFO_STREAM(nodelet_name_ << " - Initializing Depth Control Preset to " << dc_preset);
00390             ROS_DEBUG_STREAM(nodelet_name_ << " - NOTICE: Individual Depth Control values " <<
00391                 "set by params will be ignored; set r200_dc_preset=-1 to override.");
00392             rs_apply_depth_control_preset(rs_device_, dc_preset);
00393 
00394             // Save the preset value string
00395             last_dc = setDynamicReconfigDepthControlIndividuals();
00396           }
00397         }
00398       }
00399       else
00400       {
00401         // Changing individual Depth Control params means preset is Unused/Invalid
00402         // if the individual values are not the same as the preset values
00403         if (dc_preset != -1 && current_dc != last_dc)
00404         {
00405           ROS_DEBUG_STREAM(nodelet_name_ << " - Forcing Depth Control Preset to Unused");
00406           setDynamicReconfigDepthControlPreset(-1);
00407         }
00408       }
00409     }
00410     else
00411     { // Individual Depth Control not set
00412       if (bit_level.test(6))  // 2^6 = 64 : Depth Control Preset
00413       {
00414         dc_preset = config.r200_dc_preset;
00415 
00416         if (dc_preset != -1)
00417         {
00418           ROS_DEBUG_STREAM(nodelet_name_ << " - Set Depth Control Preset to " << dc_preset);
00419           rs_apply_depth_control_preset(rs_device_, dc_preset);
00420 
00421           // Save the preset value string
00422           last_dc = setDynamicReconfigDepthControlIndividuals();
00423         }
00424       }
00425     }
00426   }
00427 
00428   /*
00429    * Get the camera extrinsics
00430    */
00431   void R200Nodelet::getCameraExtrinsics()
00432   {
00433     BaseNodelet::getCameraExtrinsics();
00434 
00435     // Get offset between base frame and infrared2 frame
00436     rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED2, RS_STREAM_COLOR, &color2ir2_extrinsic_, &rs_error_);
00437     if (rs_error_)
00438     {
00439       ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
00440     }
00441     checkError();
00442   }
00443 
00444   /*
00445    * Publish Static transforms.
00446    */
00447   void R200Nodelet::publishStaticTransforms()
00448   {
00449     BaseNodelet::publishStaticTransforms();
00450 
00451     tf::Quaternion q_i2io;
00452     geometry_msgs::TransformStamped b2i_msg;
00453     geometry_msgs::TransformStamped i2io_msg;
00454 
00455     // Transform base frame to infrared2 frame
00456     b2i_msg.header.stamp = transform_ts_;
00457     b2i_msg.header.frame_id = base_frame_id_;
00458     b2i_msg.child_frame_id = frame_id_[RS_STREAM_INFRARED2];
00459     b2i_msg.transform.translation.x =  color2ir2_extrinsic_.translation[2];
00460     b2i_msg.transform.translation.y = -color2ir2_extrinsic_.translation[0];
00461     b2i_msg.transform.translation.z = -color2ir2_extrinsic_.translation[1];
00462     b2i_msg.transform.rotation.x = 0;
00463     b2i_msg.transform.rotation.y = 0;
00464     b2i_msg.transform.rotation.z = 0;
00465     b2i_msg.transform.rotation.w = 1;
00466     static_tf_broadcaster_.sendTransform(b2i_msg);
00467 
00468     // Transform infrared2 frame to infrared2 optical frame
00469     q_i2io.setRPY(-M_PI/2, 0.0, -M_PI/2);
00470     i2io_msg.header.stamp = transform_ts_;
00471     i2io_msg.header.frame_id = frame_id_[RS_STREAM_INFRARED2];
00472     i2io_msg.child_frame_id = optical_frame_id_[RS_STREAM_INFRARED2];
00473     i2io_msg.transform.translation.x = 0;
00474     i2io_msg.transform.translation.y = 0;
00475     i2io_msg.transform.translation.z = 0;
00476     i2io_msg.transform.rotation.x = q_i2io.getX();
00477     i2io_msg.transform.rotation.y = q_i2io.getY();
00478     i2io_msg.transform.rotation.z = q_i2io.getZ();
00479     i2io_msg.transform.rotation.w = q_i2io.getW();
00480     static_tf_broadcaster_.sendTransform(i2io_msg);
00481   }
00482 
00483   /*
00484    * Publish Dynamic transforms.
00485    */
00486   void R200Nodelet::publishDynamicTransforms()
00487   {
00488     tf::Transform tr;
00489     tf::Quaternion q;
00490 
00491     BaseNodelet::publishDynamicTransforms();
00492 
00493     // Transform base frame to infrared2 frame
00494     tr.setOrigin(tf::Vector3(
00495            color2ir2_extrinsic_.translation[2],
00496           -color2ir2_extrinsic_.translation[0],
00497           -color2ir2_extrinsic_.translation[1]));
00498     tr.setRotation(tf::Quaternion(0, 0, 0, 1));
00499     dynamic_tf_broadcaster_.sendTransform(tf::StampedTransform(tr, transform_ts_,
00500           base_frame_id_, frame_id_[RS_STREAM_INFRARED2]));
00501 
00502     // Transform infrared2 frame to infrared2 optical frame
00503     tr.setOrigin(tf::Vector3(0, 0, 0));
00504     q.setRPY(-M_PI/2, 0.0, -M_PI/2);
00505     tr.setRotation(q);
00506     dynamic_tf_broadcaster_.sendTransform(tf::StampedTransform(tr, transform_ts_,
00507           frame_id_[RS_STREAM_INFRARED2], optical_frame_id_[RS_STREAM_INFRARED2]));
00508   }
00509 }  // namespace realsense_camera


realsense_camera
Author(s): Rajvi Jingar , Reagan Lopez , Matt Hansen , Mark Horn
autogenerated on Thu Jun 6 2019 21:15:58