ROS x C++のお勉強

背景

PCLや画像処理の高速化などC++でしか扱えないものを使えるようになりたい。

ROSパッケージの構成検討

robonchu.hatenablog.com

最小構成(hello world)

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(hello)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES chapter03
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(hello_world src/hello_world.cpp)
target_link_libraries(hello_world ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>hello</name>
  <version>0.0.0</version>
  <description>The hello package</description>
  <maintainer email="mouse@todo.todo">mouse</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>std_msgs</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
</package>

hello_world.cpp

#include <ros/ros.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "hello_world");
  ros::NodeHandle nh;

  ros::Rate rate(1);
  while(ros::ok()){
    ROS_INFO_STREAM("Hello ROS world !!!");
    rate.sleep();
  }
  return 0;
}

ImageTransport

image_transport/Tutorials - ROS Wiki

cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages - ROS Wiki

subscriber

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
  try {
    cv::imshow("ImageSubscriber", cv_bridge::toCvShare(msg, "bgr8")->image);
    if(cv::waitKey(30) >= 0)
      ros::shutdown();
  }
  catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.",
          msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "camera_subscriber");
  ros::NodeHandle nh;
  cv::namedWindow("Image_Subscriber");
  cv::startWindowThread();

  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub;
  sub = it.subscribe("/usb_cam/image_raw", 1,imageCallback);

  ros::spin();
  cv::destroyWindow("camera_Subscriber");
  return EXIT_SUCCESS;
}

camera subscriber カメラのInfo情報が追加

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>

void imageCallback(const sensor_msgs::ImageConstPtr& msg, 
     const sensor_msgs::CameraInfoConstPtr& info)
{
  try {
    cv::imshow("ImageCameraSubscriber", cv_bridge::toCvShare(msg, "bgr8")->image);
    if(cv::waitKey(30) >= 0)
      ros::shutdown();
  }
  catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.",
          msg->encoding.c_str());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "camera_camerasubscriber");
  ros::NodeHandle nh;
  cv::namedWindow("Image_CameraSubscriber");
  cv::startWindowThread();

  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub = it.subscribe("/usb_cam/image_raw", 1,imageCallback);

  ros::spin();
  cv::destroyWindow("camera_Subscriber");

  return EXIT_SUCCESS;
}

参考：

image_transport/Tutorials/SubscribingToImages - ROS Wiki

PCL

TODO