00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 #include <ros/ros.h>
00061 #include <image_transport/image_transport.h>
00062 #include <cv_bridge/CvBridge.h>
00063
00064
00065 #include <sensor_msgs/Image.h>
00066 #include <sensor_msgs/CameraInfo.h>
00067 #include <sensor_msgs/fill_image.h>
00068 #include <sensor_msgs/SetCameraInfo.h>
00069 #include <sensor_msgs/PointCloud.h>
00070 #include <sensor_msgs/PointCloud2.h>
00071
00072 #include <cob_camera_sensors/GetTOFImages.h>
00073
00074
00075 #include <cob_camera_sensors/AbstractRangeImagingSensor.h>
00076 #include <cob_vision_utils/CameraSensorToolbox.h>
00077 #include <cob_vision_utils/GlobalDefines.h>
00078 #include <cob_vision_utils/VisionUtils.h>
00079
00080 #include <boost/thread/mutex.hpp>
00081
00082 using namespace ipa_CameraSensors;
00083
00084 class CobTofCameraNode
00085 {
00086 enum t_Mode
00087 {
00088 MODE_TOPIC = 0,
00089 MODE_SERVICE
00090 };
00091
00092 private:
00093 ros::NodeHandle node_handle_;
00094
00095 image_transport::ImageTransport image_transport_;
00096 image_transport::CameraPublisher xyz_image_publisher_;
00097 image_transport::CameraPublisher grey_image_publisher_;
00098 ros::Publisher topicPub_pointCloud_;
00099 ros::Publisher topicPub_pointCloud2_;
00100
00101 sensor_msgs::CameraInfo camera_info_msg_;
00102
00103 ros::ServiceServer camera_info_service_;
00104 ros::ServiceServer image_service_;
00105
00106 AbstractRangeImagingSensorPtr tof_camera_;
00107
00108 std::string config_directory_;
00109 int tof_camera_index_;
00110 ipa_CameraSensors::t_cameraType tof_camera_type_;
00111 bool filter_xyz_by_amplitude_;
00112 bool filter_xyz_tearoff_edges_;
00113 int lower_amplitude_threshold_;
00114 int upper_amplitude_threshold_;
00115 double tearoff_tear_half_fraction_;
00116
00117 cv::Mat xyz_image_32F3_;
00118 cv::Mat grey_image_32F1_;
00119
00120 CobTofCameraNode::t_Mode ros_node_mode_;
00121 boost::mutex service_mutex_;
00122
00123 bool publish_point_cloud_;
00124 bool publish_point_cloud_2_;
00125
00126 public:
00128 CobTofCameraNode(const ros::NodeHandle& node_handle)
00129 : node_handle_(node_handle),
00130 image_transport_(node_handle),
00131 tof_camera_(AbstractRangeImagingSensorPtr()),
00132 xyz_image_32F3_(cv::Mat()),
00133 grey_image_32F1_(cv::Mat()),
00134 publish_point_cloud_(false),
00135 publish_point_cloud_2_(false)
00136 {
00138 }
00139
00141 ~CobTofCameraNode()
00142 {
00143 tof_camera_->Close();
00144 }
00145
00148 bool init()
00149 {
00150 if (loadParameters() == false)
00151 {
00152 ROS_ERROR("[color_camera] Could not read all parameters from launch file");
00153 return false;
00154 }
00155
00156
00157 if (tof_camera_->Init(config_directory_, tof_camera_index_) & ipa_CameraSensors::RET_FAILED)
00158 {
00159
00160 std::stringstream ss;
00161 ss << "Initialization of tof camera ";
00162 ss << tof_camera_index_;
00163 ss << " failed";
00164 ROS_ERROR("[tof_camera] %s", ss.str().c_str());
00165 tof_camera_ = AbstractRangeImagingSensorPtr();
00166 return false;
00167 }
00168
00169 if (tof_camera_->Open() & ipa_CameraSensors::RET_FAILED)
00170 {
00171 std::stringstream ss;
00172 ss << "Could not open tof camera ";
00173 ss << tof_camera_index_;
00174 ROS_ERROR("[tof_camera] %s", ss.str().c_str());
00175 tof_camera_ = AbstractRangeImagingSensorPtr();
00176 return false;
00177 }
00178
00180 ipa_CameraSensors::t_cameraProperty cameraProperty;
00181 cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
00182 tof_camera_->GetProperty(&cameraProperty);
00183 int range_sensor_width = cameraProperty.cameraResolution.xResolution;
00184 int range_sensor_height = cameraProperty.cameraResolution.yResolution;
00185 cv::Size range_image_size(range_sensor_width, range_sensor_height);
00186
00188 ipa_CameraSensors::CameraSensorToolboxPtr tof_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
00189 tof_sensor_toolbox->Init(config_directory_, tof_camera_->GetCameraType(), tof_camera_index_, range_image_size);
00190
00191 cv::Mat intrinsic_mat = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_type_, tof_camera_index_);
00192 cv::Mat distortion_map_X = tof_sensor_toolbox->GetDistortionMapX(tof_camera_type_, tof_camera_index_);
00193 cv::Mat distortion_map_Y = tof_sensor_toolbox->GetDistortionMapY(tof_camera_type_, tof_camera_index_);
00194 tof_camera_->SetIntrinsics(intrinsic_mat, distortion_map_X, distortion_map_Y);
00195
00197 camera_info_service_ = node_handle_.advertiseService("set_camera_info", &CobTofCameraNode::setCameraInfo, this);
00198 image_service_ = node_handle_.advertiseService("get_images", &CobTofCameraNode::imageSrvCallback, this);
00199 xyz_image_publisher_ = image_transport_.advertiseCamera("image_xyz", 1);
00200 grey_image_publisher_ = image_transport_.advertiseCamera("image_grey", 1);
00201 if(publish_point_cloud_2_) topicPub_pointCloud2_ = node_handle_.advertise<sensor_msgs::PointCloud2>("point_cloud2", 1);
00202 if(publish_point_cloud_) topicPub_pointCloud_ = node_handle_.advertise<sensor_msgs::PointCloud>("point_cloud", 1);
00203
00204 cv::Mat d = tof_sensor_toolbox->GetDistortionParameters(tof_camera_type_, tof_camera_index_);
00205 camera_info_msg_.header.stamp = ros::Time::now();
00206 camera_info_msg_.header.frame_id = "head_tof_link";
00207
00208
00209
00210
00211
00212
00213 cv::Mat k = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_type_, tof_camera_index_);
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224 camera_info_msg_.width = range_sensor_width;
00225 camera_info_msg_.height = range_sensor_height;
00226
00227 return true;
00228 }
00229
00234 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
00235 sensor_msgs::SetCameraInfo::Response& rsp)
00236 {
00238 camera_info_msg_ = req.camera_info;
00239
00240 rsp.success = false;
00241 rsp.status_message = "[tof_camera] Setting camera parameters through ROS not implemented";
00242
00243 return true;
00244 }
00245
00247 bool spin()
00248 {
00249 boost::mutex::scoped_lock lock(service_mutex_);
00250 sensor_msgs::Image::Ptr xyz_image_msg_ptr;
00251 sensor_msgs::Image::Ptr grey_image_msg_ptr;
00252 sensor_msgs::CameraInfo tof_image_info;
00253
00254 if(tof_camera_->AcquireImages(0, &grey_image_32F1_, &xyz_image_32F3_, false, false, ipa_CameraSensors::INTENSITY_32F1) & ipa_Utils::RET_FAILED)
00255 {
00256 ROS_ERROR("[tof_camera] Tof image acquisition failed");
00257 return false;
00258 }
00259
00261
00262
00263 if(filter_xyz_tearoff_edges_) ipa_Utils::FilterTearOffEdges(xyz_image_32F3_, 0, (float)tearoff_tear_half_fraction_);
00264 if(filter_xyz_by_amplitude_) ipa_Utils::FilterByAmplitude(xyz_image_32F3_, grey_image_32F1_, 0, 0, lower_amplitude_threshold_, upper_amplitude_threshold_);
00265
00266 try
00267 {
00268 IplImage img = xyz_image_32F3_;
00269 xyz_image_msg_ptr = sensor_msgs::CvBridge::cvToImgMsg(&img, "passthrough");
00270 }
00271 catch (sensor_msgs::CvBridgeException error)
00272 {
00273 ROS_ERROR("[tof_camera] Could not convert 32bit xyz IplImage to ROS message");
00274 return false;
00275 }
00276
00277 try
00278 {
00279 IplImage img = grey_image_32F1_;
00280 grey_image_msg_ptr = sensor_msgs::CvBridge::cvToImgMsg(&img, "passthrough");
00281 }
00282 catch (sensor_msgs::CvBridgeException error)
00283 {
00284 ROS_ERROR("[tof_camera] Could not convert 32bit grey IplImage to ROS message");
00285 return false;
00286 }
00287
00289 ros::Time now = ros::Time::now();
00290 xyz_image_msg_ptr->header.stamp = now;
00291 xyz_image_msg_ptr->header.frame_id = "head_tof_link";
00292 grey_image_msg_ptr->header.stamp = now;
00293 grey_image_msg_ptr->header.frame_id = "head_tof_link";
00294
00295 tof_image_info = camera_info_msg_;
00296 tof_image_info.width = grey_image_32F1_.cols;
00297 tof_image_info.height = grey_image_32F1_.rows;
00298 tof_image_info.header.stamp = now;
00299 tof_image_info.header.frame_id = "head_tof_link";
00300
00302 xyz_image_publisher_.publish(*xyz_image_msg_ptr, tof_image_info);
00303 grey_image_publisher_.publish(*grey_image_msg_ptr, tof_image_info);
00304
00305 if(publish_point_cloud_) publishPointCloud(now);
00306 if(publish_point_cloud_2_) publishPointCloud2(now);
00307
00308 return true;
00309 }
00310
00311 void publishPointCloud(ros::Time now)
00312 {
00313 ROS_DEBUG("convert xyz_image to point_cloud");
00314 sensor_msgs::PointCloud pc_msg;
00315
00316 pc_msg.header.stamp = now;
00317 pc_msg.header.frame_id = "head_tof_link";
00318
00319 cv::Mat cpp_xyz_image_32F3 = xyz_image_32F3_;
00320 cv::Mat cpp_grey_image_32F1 = grey_image_32F1_;
00321
00322 float* f_ptr = 0;
00323 for (int row = 0; row < cpp_xyz_image_32F3.rows; row++)
00324 {
00325 f_ptr = cpp_xyz_image_32F3.ptr<float>(row);
00326 for (int col = 0; col < cpp_xyz_image_32F3.cols; col++)
00327 {
00328 geometry_msgs::Point32 pt;
00329 pt.x = f_ptr[3*col + 0];
00330 pt.y = f_ptr[3*col + 1];
00331 pt.z = f_ptr[3*col + 2];
00332 pc_msg.points.push_back(pt);
00333 }
00334 }
00335 topicPub_pointCloud_.publish(pc_msg);
00336 }
00337
00338 void publishPointCloud2(ros::Time now)
00339 {
00340 cv::Mat cpp_xyz_image_32F3 = xyz_image_32F3_;
00341 cv::Mat cpp_confidence_mask_32F1 = grey_image_32F1_;
00342
00343 sensor_msgs::PointCloud2 pc_msg;
00344
00345 pc_msg.header.stamp = now;
00346 pc_msg.header.frame_id = "head_tof_link";
00347 pc_msg.width = cpp_xyz_image_32F3.cols;
00348 pc_msg.height = cpp_xyz_image_32F3.rows;
00349 pc_msg.fields.resize(4);
00350 pc_msg.fields[0].name = "x";
00351 pc_msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00352 pc_msg.fields[1].name = "y";
00353 pc_msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00354 pc_msg.fields[2].name = "z";
00355 pc_msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00356 pc_msg.fields[3].name = "confidence";
00357 pc_msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00358 int offset = 0;
00359 for (size_t d = 0; d < pc_msg.fields.size(); ++d, offset += 4)
00360 {
00361 pc_msg.fields[d].offset = offset;
00362 }
00363 pc_msg.point_step = 16;
00364 pc_msg.row_step = pc_msg.point_step * pc_msg.width;
00365 pc_msg.data.resize (pc_msg.width*pc_msg.height*pc_msg.point_step);
00366 pc_msg.is_dense = true;
00367 pc_msg.is_bigendian = false;
00368
00369 float* f_ptr = 0;
00370 float* g_ptr = 0;
00371 int pc_msg_idx=0;
00372 for (int row = 0; row < cpp_xyz_image_32F3.rows; row++)
00373 {
00374 f_ptr = cpp_xyz_image_32F3.ptr<float>(row);
00375 g_ptr = cpp_confidence_mask_32F1.ptr<float>(row);
00376 for (int col = 0; col < cpp_xyz_image_32F3.cols; col++, pc_msg_idx++)
00377 {
00378 memcpy(&pc_msg.data[pc_msg_idx * pc_msg.point_step], &f_ptr[3*col], 3*sizeof(float));
00379 memcpy(&pc_msg.data[pc_msg_idx * pc_msg.point_step + pc_msg.fields[3].offset], &g_ptr[col], sizeof(float));
00380 }
00381 }
00382 topicPub_pointCloud2_.publish(pc_msg);
00383 }
00384
00385 bool imageSrvCallback(cob_camera_sensors::GetTOFImages::Request &req,
00386 cob_camera_sensors::GetTOFImages::Response &res)
00387 {
00388 boost::mutex::scoped_lock lock(service_mutex_);
00389
00390 try
00391 {
00392 IplImage grey_img = grey_image_32F1_;
00393 IplImage xyz_img = xyz_image_32F3_;
00394 res.greyImage = *(sensor_msgs::CvBridge::cvToImgMsg(&grey_img, "passthrough"));
00395 res.xyzImage = *(sensor_msgs::CvBridge::cvToImgMsg(&xyz_img, "passthrough"));
00396 }
00397 catch (sensor_msgs::CvBridgeException error)
00398 {
00399 ROS_ERROR("[tof_camera_type_node] Could not convert IplImage to ROS message");
00400 }
00401
00402
00403 ros::Time now = ros::Time::now();
00404 res.greyImage.header.stamp = now;
00405 res.greyImage.header.frame_id = "head_tof_link";
00406 res.xyzImage.header.stamp = now;
00407 res.xyzImage.header.frame_id = "head_tof_link";
00408 return true;
00409 }
00410
00411 bool loadParameters()
00412 {
00413 std::string tmp_string = "NULL";
00414
00416 if (node_handle_.getParam("tof_camera/configuration_files", config_directory_) == false)
00417 {
00418 ROS_ERROR("[tof_camera] Path to xml configuration for tof camera not specified");
00419 return false;
00420 }
00421
00422 ROS_INFO("Configuration directory: %s", config_directory_.c_str());
00423
00425 if (node_handle_.getParam("tof_camera/tof_camera_index", tof_camera_index_) == false)
00426 {
00427 ROS_ERROR("[tof_camera] 'tof_camera_index' (0 or 1) not specified");
00428 return false;
00429 }
00430
00432 if (node_handle_.getParam("tof_camera/tof_camera_type", tmp_string) == false)
00433 {
00434 ROS_ERROR("[tof_camera] 'tof_camera_type' not specified");
00435 return false;
00436 }
00437 if (tmp_string == "CAM_SWISSRANGER")
00438 {
00439 tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_Swissranger();
00440 tof_camera_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
00441 }
00442 else if (tmp_string == "CAM_VIRTUAL")
00443 {
00444 tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_VirtualCam();
00445 tof_camera_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
00446 }
00447 else
00448 {
00449 std::string str = "[tof_camera] Camera type '" + tmp_string + "' unknown, try 'CAM_SWISSRANGER'";
00450 ROS_ERROR("%s", str.c_str());
00451 return false;
00452 }
00453
00454 ROS_INFO("Camera type: %s_%d", tmp_string.c_str(), tof_camera_index_);
00455
00457 if (node_handle_.getParam("tof_camera/filter_xyz_by_amplitude", filter_xyz_by_amplitude_) == false)
00458 {
00459 ROS_ERROR("[tof_camera] 'filter_xyz_by_amplitude not specified");
00460 return false;
00461 }
00463 if (node_handle_.getParam("tof_camera/filter_xyz_tearoff_edges", filter_xyz_tearoff_edges_) == false)
00464 {
00465 ROS_ERROR("[tof_camera] 'filter_xyz_tearoff_edges_' not specified");
00466 return false;
00467 }
00469 if (node_handle_.getParam("tof_camera/lower_amplitude_threshold", lower_amplitude_threshold_) == false)
00470 {
00471 ROS_ERROR("[tof_camera] 'lower_amplitude_threshold' not specified");
00472 return false;
00473 }
00475 if (node_handle_.getParam("tof_camera/upper_amplitude_threshold", upper_amplitude_threshold_) == false)
00476 {
00477 ROS_ERROR("[tof_camera] 'upper_amplitude_threshold' not specified");
00478 return false;
00479 }
00481 if (node_handle_.getParam("tof_camera/tearoff_pi_half_fraction", tearoff_tear_half_fraction_) == false)
00482 {
00483 ROS_ERROR("[tof_camera] 'tearoff_pi_half_fraction' not specified");
00484 return false;
00485 }
00487 if (node_handle_.getParam("tof_camera/ros_node_mode", tmp_string) == false)
00488 {
00489 ROS_ERROR("[tof_camera] Mode for tof camera node not specified");
00490 return false;
00491 }
00492 if (tmp_string == "MODE_SERVICE")
00493 {
00494 ros_node_mode_ = CobTofCameraNode::MODE_SERVICE;
00495 }
00496 else if (tmp_string == "MODE_TOPIC")
00497 {
00498 ros_node_mode_ = CobTofCameraNode::MODE_TOPIC;
00499 }
00500 else
00501 {
00502 std::string str = "[tof_camera] Mode '" + tmp_string + "' unknown, try 'MODE_SERVICE' or 'MODE_TOPIC'";
00503 ROS_ERROR("%s", str.c_str());
00504 return false;
00505 }
00506
00507 if(node_handle_.getParam("tof_camera/publish_point_cloud", publish_point_cloud_) == false)
00508 {
00509 ROS_WARN("[tof_camera] Flag for publishing PointCloud not set, falling back to default (false)");
00510 }
00511 if(node_handle_.getParam("tof_camera/publish_point_cloud_2", publish_point_cloud_2_) == false)
00512 {
00513 ROS_WARN("[tof_camera] Flag for publishing PointCloud2 not set, falling back to default (false)");
00514 }
00515
00516
00517 ROS_INFO("ROS node mode: %s", tmp_string.c_str());
00518
00519 return true;
00520 }
00521 };
00522
00523
00524
00525
00526 int main(int argc, char** argv)
00527 {
00529 ros::init(argc, argv, "tof_camera");
00530
00532 ros::NodeHandle nh;
00533
00535 CobTofCameraNode camera_node(nh);
00536
00538 if (!camera_node.init()) return 0;
00539
00540
00541 ros::Rate rate(100);
00542 while(nh.ok())
00543 {
00544 camera_node.spin();
00545 ros::spinOnce();
00546 rate.sleep();
00547 }
00548
00549 return 0;
00550 }
