ROSでプログラミングをする際に最低限知っておくべきことのまとめ

Autowareではじめる自律移動技術入門

目的

最近、約8年ぶりにROSを使ったプログラミングをすることに
なったのですが、あまりに久しぶりすぎて基本的なことを全て
忘れてしまっていました。
今回また一から調べなおして整理したので、またいつでも
おさらいできるようにまとめておくことにします。

目次

参考書籍

今回はこれら2冊の書籍を読んで勉強しなおしました。
どちらも説明が丁寧で分かりやすかったのでお勧めです。

Autowareではじめる自律移動技術入門

実用ロボット開発のためのROSプログラミング

動作環境

今回は下記の条件を満たす環境で作業を行いました。

• Ubuntu 18.04
• ROS1 Melodic

ROS1のインストール

基本的にはこちらの公式ドキュメントにある手順に従えば
問題なくインストールできます。

wiki.ros.org

インクルードパスの設定

VSCodeでコーディングをするために、c_cpp_properties.jsonの
"includePath"をこのようにしておきます。

"includePath": [
    "/home/shisato/catkin_ws/devel/include/**",
    "/opt/ros/melodic/include/**",
    "/home/shisato/catkin_ws/src/package_name/include/**",
    "/usr/include/**"
]

作業用ワークスペースを作る

こちらのコマンドを順次実行してワークスペースを作ります。
まずは専用のディレクトリを用意します。

~$ mkdir -p ~/catkin_ws/src

次にそのディレクトリに移動して、ワークスペースの初期化を
行います。

~$ cd catkin_ws/src

~/catkin_ws/src$ catkin_init_workspace

すると、今いるcatkin_ws/srcのディレクトリ以下にCMakeLists.txtが
作成されます。

~/catkin_ws/src$ 別途
CMakeLists.txt

その後、作成したワークスペース用ディレクトリの直下の階層まで戻り、
ビルドを実行します。

~/catkin_ws$ catkin_make

そして、このように計3つのディレクトリが作られればOKです。

~/catkin_ws$ ls
build devel src

環境変数を設定する

ホームディレクトリにある.bashrcに下記の設定を追加しましょう。
こうすることで、必要な環境設定が自動で読み込まれるようになります。

source /opt/ros/melodic/setup.bash
source ~/catkin_ws/devel/setup.bash

2行目の設定はワークスペース固有のものなので、別のワークスペースで
作業する際は専用の設定をまた別途追記して、使わないワークスペースの
方の設定はコメントアウトしておきましょう。

パッケージを作る

ソースコードや各種設定ファイル、データなどをまとめておく
パッケージを作ります。ROSのコードは基本的に、このパッケージ単位で
Gitなどでバージョン管理されます。

~/catkin_ws/src$ catkin_create_pkg hoge_pkg roscpp std_msgs

このコマンドの使い方としては、
catkin_create_pkg 作成するパッケージ名 依存パッケージ名
となっています。依存パッケージについては、後でCMakeLists.txtを編集する
ことで追加できるので、この時点で必要なすべての依存パッケージを
入れないといけないわけではありません。

パッケージが作成できたら、念のためビルドが出来ることを確認して
おきましょう。このとき、ビルドコマンドを実行する場所は必ず
ワークスペースディレクトリの直下となるので間違えないように
しましょう。

~/catkin_ws/$ catkin_make

Publisherノードを作る

ROSでは、ノードと呼ばれる小さなプログラムが様々なデータをやり取り
することで目的の動作を実現します。その時、データを発信するノードを
Publisherと呼び、基本的にはこちらのようなコードを書くことで実装できます。

// header declaration
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

int main(int argc, char **argv)
{
    // setting node
    // init node name as "talker"
    ros::init(argc, argv, "talker");

    // define node handler
    // node handler is class which define publisher/subscriber
    // or handle parameter of node
    ros::NodeHandle nh_pub; // to handle publisher
    ros::NodeHandle nh_param("~"); // to handle ros parameter

    // read value of ros parameter given as input
    // input format: _ParameterName:=value
    // set value as "aaa" to sentence_
    std::string sentence_;
    nh_param.getParam("aaa", sentence_);

    // define publisher
    // set topic name as "chatter", type is std_msgs/String by advertise
    // 1000 means buffer size in topic
    ros::Publisher chatter_pub = nh_pub.advertise<std_msgs::String>("chatter", 1000);

    // loop to send data
    int count = 0;
    ros::Rate loop_rate(10); // iterate per 10Hz
    while (ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss << sentence_ << count;
        msg.data = ss.str();
        ROS_INFO("%s", msg.data.c_str()); // display info, warning, error
        chatter_pub.publish(msg); // send message
        loop_rate.sleep(); // adjust keeping 10Hz iteration
        ++count;
    }

    return 0;
}

Subscriberノードを作る

Publisherノードが発信したデータを受け取るノードをSubscriberと呼び、
こちらのようなコードで実装できます。

// header declaration
#include "ros/ros.h"
#include "std_msgs/String.h"

// callback function
// called when message was written in topic
// given pointer to message
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
    // setting node
    // init node name as "listener"
    ros::init(argc, argv, "listener");

    // define node handler to subscribe topic "chatter"
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

    // wait for a message
    ros::spin();

    return 0;
}

プログラムを実行する

作成したノードが通信できるようにするためには、roscoreを事前に
起動する必要があります。roscoreとは、ROSベースのシステムに
必要なノードとプログラムの集合体のことであり、こちらのコマンドで
起動できます。

~/catkin_ws$ roscore

そして、特定のノードを起動するにはこちらのコマンドを実行します。

~/catkin_ws$ rosrun パッケージ名 ノード名

独自のメッセージ型を作成する

ROSでは標準のメッセージ型がいろいろ用意されていますが、
下記の手順に従って独自の型を作ることができます。
まずは、そのメッセージ型を定義する専用のパッケージを作成します。

~/catkin_ws/src $ catkin_create_pkg original_msgs

そして、パッケージのディレクトリ内にmsgディレクトリを作り、
そこに独自の型を定義した.msgファイルを作成して置くようにします。

ここで作成したmy.msgファイルには、例えばこのようにメッセージに
含むデータの内容を記述します。

int32[] my_array
float64 my_double

package.xmlには、定義したメッセージ型のビルドや実行をするのに
必要なこちらの設定を追記します。

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<buildtool_depend>catkin</buildtool_depend>

CMakeLists.txtには、ビルドに必要な下記の依存関係を追記します。

find_package(catkin REQUIRED message_generation)

## Generate messages in the 'msg' folder
add_message_files(
   FILES
   my.msg
)

## Generate added messages and services with any dependencies listed here
generate_messages(
   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
)

catkin_package(
  CATKIN_DEPENDS message_runtime
)

ここまで出来たらビルドを実行します。

~/catkin_ws$ catkin_make

すると、こちらのディレクトリに作成した独自メッセージの
ヘッダファイルが作成されていることを確認できます。

~/catkin_ws$ ls devel/include/original_msgs/
my.h

独自メッセージを使って通信するノードを作成する

先程作成した独自メッセージのパッケージを依存関係に
加えたパッケージを作成します。

~/catkin_ws/src$ catkin_create_pkg パッケージ名 original_msgs roscpp

次にPublisherノードを作成します。

// header declaration
#include "ros/ros.h"
#include "test_msgs/my.h"

int main(int argc, char **argv)
{
    // setting node
    // init node name as "publisher"
    ros::init(argc, argv, "publisher");

    // define node handler
    // node handler is class which define publisher/subscriber
    // or handle parameter of node
    ros::NodeHandle nh_pub; // to handle publisher

    // define publisher
    ros::Publisher chatter_pub = nh_pub.advertise<test_msgs::my>("chatter", 1000);

    // loop to send data
    test_msgs::my msg;
    int count = 0;
    ros::Rate loop_rate(10); // iterate per 10Hz
    while (ros::ok())
    {
        msg.my_array.push_back(count);
        msg.my_double = (double)count;
        chatter_pub.publish(msg); // send message
        loop_rate.sleep(); // adjust keeping 10Hz iteration
        ++count;
    }

    return 0;
}

続いてSubscriberを作成します。

// header declaration
#include "ros/ros.h"
#include "test_msgs/my.h"

// callback function
// called when message was written in topic
// given pointer to message
void chatterCallback(const test_msgs::my::ConstPtr& msg)
{
    ROS_INFO("array size: %d", (int)msg->my_array.size());
    ROS_INFO("I heard : %f", msg->my_double);
}

int main(int argc, char **argv)
{
    // setting node
    ros::init(argc, argv, "subscriber");

    // define node handler to subscribe topic "chatter"
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

    // wait for a message
    ros::spin();

    return 0;
}

これらをビルドするためにCMakeLists.txtを編集します。

## Target nodes and directories
add_executable(publisher src/publisher.cpp)
target_link_libraries(publisher ${catkin_LIBRARIES})
add_executable(subscriber src/subscriber.cpp)
target_link_libraries(subscriber ${catkin_LIBRARIES})

あとは、ここまでに述べた通りの手順でビルドし、それぞれの
ノードを実行、通信ができればOKです。

tfで座標変換する

tfは各座標系の連鎖関係を管理するシステムです。
例えば下記のような異なる座標系のつながりを表現することができます。

• 世界座標系: world
• 地図座標系: map
• ロボット座標系: base_link
• センサ座標系: velodyne

tfを扱うためには、まずパッケージの依存関係にtfを追加する必要があります。

~/catkin_ws/src$ catkin_create_pkg パッケージ名 tf turtlesim roscpp

例えば、ロボットがセンサで何か障害物を検知しているとして、
ロボット座標系における障害物の位置を取得したいとします。
そしてこの時、ロボットと障害物は共通の地図座標系上にいるものと
します。

この場合まずは、地図座標系(map)とロボット座標系(base_link)、
地図座標系(map)と障害物座標系(obstacle)の連鎖関係をそれぞれ
定義して発信するbroadcasterクラスを作ります。
このコードでは、既にpublishされているロボットの位置座標データを
subscribeし、その際のコールバック処理として上記の連鎖関係の定義と
broadcastを実行しています。

// header
#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "turtlesim/Pose.h"

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // declare variables for tf
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion q;

    // set origin and axis direction of base_link coordinate
    // on map coordinate
    // set pose on map coordinate from turtlesim_node as origin
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    q.setRPY(0, 0, msg->theta); // set roll, pitch, yaw of turtle
    transform.setRotation(q);
    // input pose on base_link coordinate on map coordinate
    // parent:map, child:base_link
    // send pose at current time
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "base_link"));

    // set origin and axis direction of obstacle coordinate
    // on map coordinate
    transform.setOrigin(tf::Vector3(2.0, 2.0, 0.0));
    q.setRPY(0, 0, 0);
    transform.setRotation(q);
    // input pose on obstacle coordinate on map coordinate
    // parent:map, child:obstacle
    // send pose at current time
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "obstacle"));
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tf_broadcaster");
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe("turtle1/pose", 10, &poseCallback);
    ros::spin();
    return 0;
}

続いて、broadcastされた座標系の連鎖関係に基づいて、障害物位置を
ロボット座標系に変換するlistenerノードを作成します。

// header
#include "ros/ros.h"
#include "tf/transform_listener.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tf_listener");
    double x, y;
    ros::NodeHandle node;
    tf::TransformListener listener; // listener class
    ros::Rate rate(10.0);

    while (node.ok())
    {
        tf::StampedTransform transform;

        // receive pose on obstacle coordinate from base_link coordinate
        try {
            // calculate pose on 2nd input coordinate from 1st input coordinate
            // at 3rd input time and calculated pose is stored in 4th input
            // ros::Time(0) means the latest time
            listener.lookupTransform("/base_link", "/obstacle", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
            // when both of coordinates don't exist at input time,
            // tf report exception
            ROS_ERROR("%s", ex.what());
            continue;
        }

        // get position on obstacle coordinate from base_link coordinate
        x = transform.getOrigin().x();
        y = transform.getOrigin().y();
        if (0<x && x<2 && -1<y && y<1) {
            ROS_WARN("danger (%f, %f)", x, y);
        }
        else {
            ROS_INFO("(%f, %f)", x, y);
        }
        
        rate.sleep();
    }

    return 0;
}

rqt_graphでノード間のつながりを確認する

各ノードがデータをやり取りする際は、そのデータを入れ物である
メッセージの型に格納します。そしてそれをトピックという名前を
付けたうえでpublish-subscribeを行います。
rqt-graphは、こちらのように各ノードが正しいトピック名によって
接続されているかどうかを可視化してくれる便利なツールです。

rostopicでトピックの内容を確認する

各ノードが通信しているトピック名や、それに該当するメッセージの
型や内容をこのように確認することができます。

使い方は大きく分けて2種類あり、メッセージの型を確認するための
rostopic type トピック名、メッセージの内容を確認するための
rostopic echo トピック名、があります。

rvizで可視化する

rvizを使うことで、ロボットの動きやセンサデータなどを可視化する
ことができます。可視化するデータを追加するときは、画面左下の
Addボタンを押し、そのデータの型に該当するものを選択します。

また画面右側では、登録されてるtfの座標系を選択することで、
それを中心とした表示に切り替えることができます。

マウスを使って視点を自由に変えられますが、このZeroボタンを
押すと、選択されている座標系を中心とした表示にリセットする
ことができます。

ユニットテストを実行する

ROSではユニットテストを行う仕組みとしてrostestが提供されています。
rostestは既に説明したroslaunchの拡張であり、同様の方法でシステムの
セットアップ、テストの実行ができるようになっています。

このときに作成するテストコードは、C++の場合ならgtestを利用します。

まずは、テストをする対象のコードを作成します。例えば下記のような
関数群を実装したとして、これらをユニットテストできるようにします。

/**
 * @file grid_map_utility.hpp
 * @author Shisato Yano (shisatoyano@gmail.com)
 * @brief Common methods for Girid map calculation
 * @version 0.1
 * @date 2022-06-30
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef _H_GRID_MAP_UTILITY_
#define _H_GRID_MAP_UTILITY_

#include <ros/ros.h>

/**
 * @brief Calculate index X in 2D grid map from x position[m]
 * 
 * @param x position x[m] of point cloud
 * @param origin_x position x[m] of map origin (lower left)
 * @param resolution [m/cell]
 * @return uint32_t 
 */
uint32_t getGridIndexX(_Float32 x, _Float32 origin_x, _Float32 resolution)
{
    return static_cast<uint32_t>((x - origin_x)/resolution);
}

/**
 * @brief Calculate index y in 2D grid map from y position[m]
 * 
 * @param y position y[m] of point cloud
 * @param origin_y position y[m] of map origin (lower left)
 * @param resolution [m/cell]
 * @return uint32_t 
 */
uint32_t getGridIndexY(_Float32 y, _Float32 origin_y, _Float32 resolution)
{
    return static_cast<uint32_t>((y - origin_y)/resolution);
}

/**
 * @brief Calculate index in grid map as 1D vector from index x-y
 * each index is sorted as row major order
 * @param i_x index x of 2D grid map
 * @param i_y index y of 2D grid map
 * @param height number of cell in height direction
 * @return uint32_t 
 */
uint32_t getVectorIndexRowMajorOrder(uint32_t i_x, uint32_t i_y, uint32_t height)
{
    return i_y * height + i_x;
}

/**
 * @brief Calculate index x in 2D grid map from 1D vector
 * 1D vector is sorted as row major order
 * @param i_v index of 1D vector
 * @param height number of cell in height direction
 * @return uint32_t 
 */
uint32_t getGridIndexXFromVectorIndex(uint32_t i_v, uint32_t height)
{
    return i_v % height;
}

/**
 * @brief Calculate index y in 2D grid map from 1D vector
 * 1D vector is sorted as row major order
 * @param i_v index of 1D vector
 * @param height number of cell in height direction
 * @return uint32_t 
 */
uint32_t getGridIndexYFromVectorIndex(uint32_t i_v, uint32_t height)
{
    return i_v / height;
}

/**
 * @brief Calculate x position[m] from index x in 2D grid map
 * 
 * @param i_x index x of 2D grid map
 * @param origin_x position x[m] of map origin (lower left)
 * @param resolution [m/cell]
 * @return _Float32 
 */
_Float32 getXFromGridIndexX(uint32_t i_x, _Float32 origin_x, _Float32 resolution)
{
    return static_cast<_Float32>(i_x) * resolution + origin_x;
}

/**
 * @brief Calculate y position[m] from index y in 2D grid map
 * 
 * @param i_y index y of 2D grid map
 * @param origin_y position y[m] of map origin (lower left)
 * @param resolution [m/cell]
 * @return _Float32 
 */
_Float32 getYFromGridIndexY(uint32_t i_y, _Float32 origin_y, _Float32 resolution)
{
    return static_cast<_Float32>(i_y) * resolution + origin_y;
}

/**
 * @brief Validate index x is within width of grid map
 * 
 * @param i_x index x of 2D grid map
 * @param width number of cell in width direction
 * @return true 
 * @return false 
 */
bool isGridIndexXValid(uint32_t i_x, uint32_t width)
{
    if ((i_x < 0) || (i_x >= width)) return false;
    return true;
}

/**
 * @brief Validate index y is within height of grid map
 * 
 * @param i_y index y of 2D grid map
 * @param height number of cell in height direction
 * @return true 
 * @return false 
 */
bool isGridIndexYValid(uint32_t i_y, uint32_t height)
{
    if ((i_y < 0) || (i_y >= height)) return false;
    return true;
}

/**
 * @brief Validate index is within length of 1D vector
 * 
 * @param i index of grid map sorted as 1D vector
 * @param all_cell_num all number of cell
 * @return true 
 * @return false 
 */
bool isVectorIndexValid(uint32_t i, uint32_t all_cell_num)
{
    if ((i < 0) || (i >= all_cell_num)) return false;
    return true;
}

#endif // _H_GRID_MAP_UTILITY_

次に、上記の関数群のユニットテストコードを実装します。
基本的にはこのように、TESTマクロによって指定して範囲を
一つのテストケースとして作成します。

TEST(GridMapUtilityTest, GridIndexX)
{
    ASSERT_EQ(35, getGridIndexX(3.0, origin_x_m, RESOLUTION_M));
}

TESTマクロの第一引数はテストスイートといい、複数のテストケースを
まとめてカテゴライズしたものです。第二引数はテストケースの名称
です。
そして、この指定された範囲の中にテスト時の具体的な処理を
記述していきますが、最も基本的なやり方は上記のようにASSERT_EQと
いうマクロを使うやり方です。これは、第一引数と第二引数の値が一致
すればPass、そうでなければFaileと判定するものです。

以上を踏まえて、今回はこのように必要なテストケースを一通り
作成しておきます。

#include <gtest/gtest.h>
#include <ros/ros.h>

#include "grid_map_utility.hpp"

_Float32 RESOLUTION_M = 0.2;
_Float32 HEIGHT_M = 10.0;
_Float32 WIDTH_M = 10.0;
_Float32 CENTER_X_M = 1.0;
_Float32 CENTER_Y_M = 0.0;

_Float32 origin_x_m = CENTER_X_M - (WIDTH_M/2);
_Float32 origin_y_m = CENTER_Y_M - (HEIGHT_M/2);

TEST(GridMapUtilityTest, GridIndexX)
{
    ASSERT_EQ(35, getGridIndexX(3.0, origin_x_m, RESOLUTION_M));
}

TEST(GridMapUtilityTest, GridIndexY)
{
    ASSERT_EQ(30, getGridIndexY(1.0, origin_y_m, RESOLUTION_M));
}

TEST(GridMapUtilityTest, VectorIndexRowMajorOrder)
{
    ASSERT_EQ(1535, getVectorIndexRowMajorOrder(35, 30, (HEIGHT_M/RESOLUTION_M)));
}

TEST(GridMapUtilityTest, GridIndexXFromVectorIndex)
{
    ASSERT_EQ(35, getGridIndexXFromVectorIndex(1535, (HEIGHT_M/RESOLUTION_M)));
}

TEST(GridMapUtilityTest, GridIndexYFromVectorIndex)
{
    ASSERT_EQ(30, getGridIndexYFromVectorIndex(1535, (HEIGHT_M/RESOLUTION_M)));
}

TEST(GridMapUtilityTest, XFromGridIndexX)
{
    ASSERT_EQ(3.0, getXFromGridIndexX(35, origin_x_m, RESOLUTION_M));
}

TEST(GridMapUtilityTest, YFromGridIndexY)
{
    ASSERT_EQ(1.0, getYFromGridIndexY(30, origin_y_m, RESOLUTION_M));
}

TEST(GridMapUtilityTest, GridIndexXIsValid)
{
    ASSERT_EQ(true, isGridIndexXValid(49, (WIDTH_M/RESOLUTION_M)));
    ASSERT_EQ(false, isGridIndexXValid(50, (WIDTH_M/RESOLUTION_M)));
}

TEST(GridMapUtilityTest, GridIndexYIsValid)
{
    ASSERT_EQ(true, isGridIndexYValid(49, (HEIGHT_M/RESOLUTION_M)));
    ASSERT_EQ(false, isGridIndexYValid(50, (HEIGHT_M/RESOLUTION_M)));
}

TEST(GridMapUtilityTest, VectorIndexIsValid)
{
    ASSERT_EQ(true, isVectorIndexValid(2499, (WIDTH_M/RESOLUTION_M)*(HEIGHT_M/RESOLUTION_M)));
    ASSERT_EQ(false, isVectorIndexValid(2500, (WIDTH_M/RESOLUTION_M)*(HEIGHT_M/RESOLUTION_M)));
}

テストケースを一通り実装できたら、あとはそれらを実行する
ノードをこのように実装します。

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "grid_map_utility_test");

    ros::AsyncSpinner spinner(1);
    spinner.start();
    int ret = RUN_ALL_TESTS();
    spinner.stop();
    ros::shutdown();
}

RUN_ALL_TESTSマクロを利用して全テストケースを実行し、
ros::AsyncSpinnerのstart()とstop()で囲った処理を一回だけ
実行するというようにしています。

以上でテストコードの実装は完了したので、次はテスト環境の
セットアップとテスト実行を行うためのrostestファイル作成
を次のように作成します。

<launch>
    <test test-name="grid_map_utility_test" pkg="xcmg_programming_test" type="grid_map_utility_test" time-limit="5.0"/>
</launch>

ファイル名は～.testとし、タグ内でテストのノードに
関する情報を指定し、time-limit属性によってタイムアウトの
時間を指定できます。

ここで作成したテストコードとrostestファイルは、このように
testディレクトリを作成して、そこに置くようにするのが一般的です。

続いて、package.xmlも編集します。rostestを使用するには、
package.xmlにてパッケージの依存関係を定義しないといけません。
ここでは、<test_depend>タグを利用し下記のように追記します。

<test_depend>rostest</test_depend>

最後に、テストコードをビルドできるようにCMakeLists.txtを
下記を追記します。

if(CATKIN_ENABLE_TESTING)
  find_package(rostest REQUIRED)

  add_rostest_gtest(grid_map_utility_test test/grid_map_utility_test.test test/grid_map_utility_test.cpp)
  target_link_libraries(grid_map_utility_test ${catkin_LIBRARIES})
endif()

add_rostest_gtest関数では、テストコードを実行するノード名、
rostestファイル、テストコードを設定しておきます。

ここまでの準備が完了したら、下記のコマンドによりテストを
実行できます。

~/catkin_ws$ catkin_make run_tests

実行した結果、このように一通りのテストが成功すればOKです。