src/obstacle_detector.cpp
/*!
 * @if jp
 *
 * @brief callback function at sbscribing PC2 data
 *
 * - Convert the PC2 data to depth and RGB images
 * - Exec obstacle detection
 * - Publish objstacle information as ClassifiledObstacle
 *
 * @param[in] pc2 Organized PC2 data
 * @return
 *
 * @else
 * @endif
 */
@if jp
@brief callback function at sbscribing PC2 data

- Convert the PC2 data to depth and RGB images
- Exec obstacle detection
- Publish objstacle information as ClassifiledObstacle

@param[in] pc2 Organized PC2 data
@return

@else
@endif
@if jp @brief callback function at sbscribing PC2 data - Convert the PC2 data to depth and RGB images - Exec obstacle detection - Publish objstacle information as ClassifiledObstacle @param [ in ] pc2 Organized PC2 data @return @else @endif
void emulated_srs::ObstacleDetector::pc2Callback(
    const sensor_msgs::PointCloud2ConstPtr &pc2)
{
  bool bret;
  int object_count;
  std::vector<eSRS::ObstacleClassified> obstacle_classified;
  ROS_INFO_ONCE("PC2 received: %d %d", pc2->width, pc2->height);
  if(pc2->height <= 1 || pc2->width <= 1)
  {
    ROS_WARN("An unorganized PC2 has subscribed: %d x %d", 
             pc2->width, pc2->height);
    return;
  }
  if(pc2->point_step != 32 && pc2->point_step != 16)
  {
    ROS_WARN("Unsupported point_step: %d", 
             pc2->point_step);
    return;
  }
  if (flg_initialized_p_ == false)
  {
    initializeMap(pc2->width, pc2->height, pc2->point_step);
    ROS_INFO_ONCE("initialize completed: %d x %d (%d)",
                  pc2->width, pc2->height, pc2->point_step);
  }
  bret = convertPC2ToMapData(pc2);
  if (bret == false)
  {
    return;
  }
  ROS_INFO_ONCE("data conversion completed");
  header_pointcloud2_ = pc2->header;
  basename_to_save_images_ = getLocalTimeString(header_pointcloud2_.stamp) + ".png";
  maskMap();
  ROS_INFO_ONCE("masking completed");
  object_count = execObstacleDetection();
  ROS_INFO_ONCE("obstacle detection completed");
  map_for_detection_.getObstacleClassified(obstacle_classified);
  ROS_INFO_ONCE("list prepared");
  timestamp_detection_result_published_ = ros::Time::now();
  displayAll();
  publishAll(obstacle_classified, object_count);
  ROS_INFO_ONCE("published");
  return;
}
void emulated_srs :: ObstacleDetector :: pc2Callback ( const sensor_msgs :: PointCloud2ConstPtr & pc2 ) { bool bret ; int object_count ; std :: vector < eSRS :: ObstacleClassified > obstacle_classified ; ROS_INFO_ONCE ( " " , pc2 -> width , pc2 -> height ) ; if ( pc2 -> height <= 1 || pc2 -> width <= 1 ) { ROS_WARN ( " " , pc2 -> width , pc2 -> height ) ; return ; } if ( pc2 -> point_step != 32 && pc2 -> point_step != 16 ) { ROS_WARN ( " " , pc2 -> point_step ) ; return ; } if ( flg_initialized_p_ == false ) { initializeMap ( pc2 -> width , pc2 -> height , pc2 -> point_step ) ; ROS_INFO_ONCE ( " " , pc2 -> width , pc2 -> height , pc2 -> point_step ) ; } bret = convertPC2ToMapData ( pc2 ) ; if ( bret == false ) { return ; } ROS_INFO_ONCE ( " " ) ; header_pointcloud2_ = pc2 -> header ; basename_to_save_images_ = getLocalTimeString ( header_pointcloud2_ . stamp ) + " " ; maskMap ( ) ; ROS_INFO_ONCE ( " " ) ; object_count = execObstacleDetection ( ) ; ROS_INFO_ONCE ( " " ) ; map_for_detection_ . getObstacleClassified ( obstacle_classified ) ; ROS_INFO_ONCE ( " " ) ; timestamp_detection_result_published_ = ros :: Time :: now ( ) ; displayAll ( ) ; publishAll ( obstacle_classified , object_count ) ; ROS_INFO_ONCE ( " " ) ; return ; }
@if jp
 @brief callback function at sbscribing PC2 data
@if jp @brief callback function at sbscribing PC2 data
//timestamp_pointcloud2_subscribed_ = ros::Time::now(); // Mapオブジェクトの初期化と初期設定を行う。 // このコールバック関数で初めてサブスクライブした // 画像のサイズがわかるため、このタイミングで初期化が必要 //pointcloud2からデータを抜き取り画像データ深度画像とrgb画像を作る。 //失敗した原因については各関数内でROS_WARN(),ROS_ERROR()で出力している //ので、ここでは出力しない // timestamp and frame_id // apply masking to the depth MAP image // exec obect detection //map_for_detection_.display("Det", -1); // make obstacle information for publishing // exec displaying and saving // exec publishing