ROS講座83 camera_infoをセットするノード

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
uvc_cameraなどのWebカメラを扱うノードではそのカメラのゆがみなどのパラメーターを扱えます。
しかし標準のROSノードではこれは以下のようにセットします。

```xml:cameraパラメーターの読み込み
<node name="uvc_camera_node" pkg="uvc_camera" type="uvc_camera_node">
  <param name="camera_info_url" value="file://$(find cam_lecture)/config/camera.yaml"/>
</node>
```

rosparamで読んでいるのはcamera_infoのファイルのURIだけで、標準で的な上記の例ではuvc_camera_nodeが起動しているPCのローカルから読み出します。つまり分散launchなどで実行するマシンを変えるとcamera_infoファイルが読めなくなる可能性があります。

今回は一度パラメーター事態ををrosparamで読みだして、それを`set_camera_info`というrosserviceでuvc_camera_nodeにセットするROSノードを製作します。

# ソースコード

## ROSノード

```c++:cam_lecture/src/cam_call_camera_info.cpp
#include <ros/ros.h>
#include <sensor_msgs/SetCameraInfo.h>

bool get_camera_info(sensor_msgs::CameraInfo& info, ros::NodeHandle pn){
  //basic config
  std::string camera_name;
  int image_width, image_height;
  //camera config
  int camera_matrix_rows, camera_matrix_cols;
  std::vector<double>camera_matrix_data;
  //distortion config
  std::string distortion_model;
  int distortion_rows, distortion_cols;
  std::vector<double>distortion_data;
  //rectification config
  int rectification_rows, rectification_cols;
  std::vector<double>rectification_data;
  //projection config
  int projection_rows, projection_cols; 
  std::vector<double>projection_data;
  
  if(!pn.getParam("camera_name", camera_name))return false;
  if(!pn.getParam("image_width", image_width))return false;
  if(!pn.getParam("image_height", image_height))return false;

  if(!pn.getParam("camera_matrix/rows", camera_matrix_rows))return false;
  if(!pn.getParam("camera_matrix/cols", camera_matrix_cols))return false;
  if(!pn.getParam("camera_matrix/data", camera_matrix_data))return false;

  if(!pn.getParam("distortion_model", distortion_model))return false;
  if(!pn.getParam("distortion_coefficients/rows", distortion_rows))return false;
  if(!pn.getParam("distortion_coefficients/cols", distortion_cols))return false;
  if(!pn.getParam("distortion_coefficients/data", distortion_data))return false;
  
  if(!pn.getParam("rectification_matrix/rows", rectification_rows))return false;
  if(!pn.getParam("rectification_matrix/cols", rectification_cols))return false;
  if(!pn.getParam("rectification_matrix/data", rectification_data))return false;

  if(!pn.getParam("projection_matrix/rows", projection_rows))return false;
  if(!pn.getParam("projection_matrix/cols", projection_cols))return false;
  if(!pn.getParam("projection_matrix/data", projection_data))return false;

  info.header.frame_id=camera_name;
  info.width=image_width;
  info.height=image_height;

  if(camera_matrix_rows==3 && camera_matrix_cols==3 && camera_matrix_data.size()==9){
    for(int i=0;i<9;i++)info.K[i]=camera_matrix_data[i];
  }
  else return false;

  if(distortion_rows==1 && distortion_cols==5 && distortion_data.size()==5){
    info.D=distortion_data;
  }
  else return false;
  info.distortion_model=distortion_model;

  if(rectification_rows==3 && rectification_cols==3 && rectification_data.size()==9){
    for(int i=0;i<9;i++)info.R[i]=rectification_data[i];
  }
  else return false;

  if(projection_rows==3 && projection_cols==4 && projection_data.size()==12){
    for(int i=0;i<12;i++)info.P[i]=projection_data[i];
  }
  else return false;

  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "cam_call_camera_info");
  ros::NodeHandle n;
  ros::NodeHandle pn("~"); 

  ros::Duration(0.5).sleep();
  sensor_msgs::CameraInfo info;
  if(!get_camera_info(info, pn)){
    ROS_ERROR("Can not find rosparam");
    return -1;
  }

  ros::ServiceClient client = n.serviceClient<sensor_msgs::SetCameraInfo>("/set_camera_info");
  sensor_msgs::SetCameraInfo srv;
  srv.request.camera_info=info;
  if(client.call(srv) && srv.response.success){
    ROS_INFO("Success: set_camera_info");    
  }
  else{
    ROS_ERROR("Fail: set_camera_info");
    return -1;
  }
  return 0;
}
```

`get_camera_info()`関数ではrosparamからcamera_infoの設定を読み取ります。それをrosserviceでcallします。

## CMakeList

```cmake:cam_lecture/CMakeLists.txtに追加
add_executable(cam_call_camera_info src/cam_call_camera_info.cpp)

target_link_libraries(cam_call_camera_info
  ${catkin_LIBRARIES}
)
```

## launch

```xml:cam_lecture/launch/call_camera_info.launch
<?xml version="1.0"?>
<launch>
  <node name="uvc_camera_node" pkg="uvc_camera" type="uvc_camera_node" output="screen">
    <param name="camera_info_url" value="file://$(find cam_lecture)/config/camera.yaml"/>
  </node>

  <node name="cam_call_camera_info" pkg="cam_lecture" type="cam_call_camera_info" output="screen" required="true">
    <rosparam command="load" file="$(find cam_lecture)/config/camera.yaml" />
  </node>
</launch>
```


# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行

```shell
roslaunch cam_lecture call_camera_info.launch 
```

# 参考
[rosservice](http://wiki.ros.org/ja/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29)
[sensor_msgs::SetCameraInfo](http://docs.ros.org/api/sensor_msgs/html/srv/SetCameraInfo.html)
# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)