アプリとサービスのすすめ

アプリやIT系のサービスを中心に書いていきます。たまに副業やビジネス関係の情報なども気ままにつづります

Jetson Nano上のRealSense-Lidar(L515)とホストPCをROSで遠隔通信してみた

今回はJetson Nano上でRealsense Lidar(L515)を動かして、遠隔通信でデータをホストPCに送るまでの備忘録

環境

ubuntu 18.04.5 LTS
CMake 3.10.2
Python 3.6.9
Opencv 4.1.1
JetPack 6.0

作業全体構成

f:id:trafalbad:20220118235219p:plain


目次
1. Librealsenseのinstall
2. Opencvシンボリックリンク作成
3. Jetson nanoにrealsense-rosのinstall
4. Jetson上でLidarの動作確認
5. rostopic list一覧
6. ホストPCとJetsonを遠隔通信




1. Librealsenseのinstall

前回記事でinstall済みだけど一応installコマンド

# public keyの登録
sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

# サーバーレポジトリのlistの登録
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

#  SDKのinstall
sudo apt-get install librealsense2-utils
sudo apt-get install librealsense2-dev

2. Opencvシンボリックリンク作成


Jetson Nanoではopencvのパスが/usr/include/opencv4/opencv2になってるのでシンボリックリンクを作成する必要がある。

sudo ln -s /usr/include/opencv4 /usr/include/opencv

3. Jetson nanoにrealsense-rosのinstall

とにかく苦労したところ。Jetsonはarchがarmなので、ホストPCと入れ方が違うので注意

Installed at 2022/01/17

# 必要パッケージのinstall
sudo apt-get install ros-melodic-realsense2-camera
sudo apt-get install ros-melodic-realsense2-description

cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1`
#### Note:Switching to '2.3.2' 
#### HEAD is now at f400d68 2.3.2
# catkin make
cd ~/catkin_ws
catkin_make clean
>>>
### -- Build files have been written to: /home/jetson/catkin_ws/build
####
#### Running command: "make clean -j4 -l4" in "/home/jetson/catkin_ws/build"
####

catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
>>>
〜〜
#### [100%] Linking CXX shared library /home/jetson/catkin_ws/devel/lib/librealsense2_camera.so
#### [100%] Built target realsense2_camera

catkin_make install
>>>
〜〜
#### -- Installing: /home/jetson/catkin_ws/install/share/realsense2_camera/rviz/pointcloud.rviz
#### -- Installing: /home/jetson/catkin_ws/install/share/realsense2_camera/nodelet_plugins.xml
source ~/.bashrc
# 再起動
sudo reboot


installできた。

4. Jetson上でLidarの動作確認

試しにjetson内で動作を試す。

# 1つ目のterminalで
$ roslaunch realsense2_camera rs_camera.launch
# 2つ目で
$ rosrun image_view image_view image:=/camera/color/image_raw

f:id:trafalbad:20220120191201p:plain
写った。(写真はモザイクかけてあります)



5. rostopic list一覧

ホストPCと同じ。opencvシンボリックリンクを忘れるとエラーだけじゃなく、rgb画像が読めなくなる。

$ rostopic list
#### /camera/color/camera_info
#### /camera/color/image_raw
#### /camera/color/image_raw/compressed
#### /camera/color/image_raw/compressed/parameter_descriptions
#### /camera/color/image_raw/compressed/parameter_updates
#### /camera/color/image_raw/compressedDepth
#### /camera/color/image_raw/compressedDepth/parameter_descriptions
#### /camera/color/image_raw/compressedDepth/parameter_updates
#### /camera/color/image_raw/theora
#### /camera/color/image_raw/theora/parameter_descriptions
#### /camera/color/image_raw/theora/parameter_updates
#### /camera/color/metadata
#### /camera/depth/camera_info
#### /camera/depth/image_rect_raw
#### /camera/depth/image_rect_raw/compressed
#### /camera/depth/image_rect_raw/compressed/parameter_descriptions
#### /camera/depth/image_rect_raw/compressed/parameter_updates
#### /camera/depth/image_rect_raw/compressedDepth
#### /camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
#### /camera/depth/image_rect_raw/compressedDepth/parameter_updates
#### /camera/depth/image_rect_raw/theora
#### /camera/depth/image_rect_raw/theora/parameter_descriptions
#### /camera/depth/image_rect_raw/theora/parameter_updates
#### /camera/depth/metadata
#### /camera/extrinsics/depth_to_color
#### /camera/l500_depth_sensor/parameter_descriptions
#### /camera/l500_depth_sensor/parameter_updates
#### /camera/motion_module/parameter_descriptions
#### /camera/motion_module/parameter_updates
#### /camera/realsense2_camera_manager/bond
#### /camera/rgb_camera/parameter_descriptions
#### /camera/rgb_camera/parameter_updates
#### /diagnostics
#### /rosout
#### /rosout_agg
#### /tf
#### /tf_static

6. ホストPCとJetsonを遠隔通信

お試しようなので、Lidarに写った画像の中の青と赤の領域の値を表示するscript。

subscriber用pythonスクリプト(color_vel.py)

#!/usr/bin/env python3

import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError

LIDAR_NODE_NAME = '/camera/color/image_raw'
class ColorExtract(object):
    def __init__(self):
        self._vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self._blue_pub = rospy.Publisher('blue_image', Image, queue_size=1)
        self._red_pub = rospy.Publisher('red_image', Image, queue_size=1)
        self._image_sub = rospy.Subscriber(LIDAR_NODE_NAME, Image, self.callback)
        self._bridge = CvBridge()
        self._vel = Twist()

    def get_colored_area(self, cv_image, lower, upper):
        hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        mask_image = cv2.inRange(hsv_image, lower, upper)
        extracted_image = cv2.bitwise_and(cv_image, cv_image, mask=mask_image)
        area = cv2.countNonZero(mask_image)
        return (area, extracted_image)
        
    def callback(self, data):
        try:
            cv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8')
        except CvBridgeError as e:
            print(e)
        blue_area, blue_image = self.get_colored_area(
            cv_image, np.array([50,100,100]), np.array([150,255,255]))
        red_area, red_image = self.get_colored_area(
            cv_image, np.array([150,100,150]), np.array([180,255,255]))
            
        try:
            self._blue_pub.publish(self._bridge.cv2_to_imgmsg(blue_image, 'bgr8'))
            self._red_pub.publish(self._bridge.cv2_to_imgmsg(red_image, 'bgr8'))
        except CvBridgeError as e:
            print(e)
        rospy.loginfo('blue=%d, red=%d' % (blue_area, red_area))
        if blue_area > 10:
            self._vel.linear.x = 0.5
            self._vel_pub.publish(self._vel)
        if red_area > 10:
            self._vel.linear.x = -0.5
            self._vel_pub.publish(self._vel)


if __name__ == '__main__':
    rospy.init_node('color_extract')
    color = ColorExtract()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        pass

ホストPCとjetsonで遠隔通信

# ホストPC側の環境設定
export ROS_MASTER_URI=http://192.168.10.111:11311  # ROS_MASTER側のアドレスを設定
export ROS_IP=192.168.10.111                      # 自分のIPアドレスを設定


# jetson側の環境設定
export ROS_MASTER_URI=http://192.168.10.111:11311
export ROS_IP=192.168.10.102
# Jetson側で実行コマンド
$ roslaunch realsense2_camera rs_camera.launch
# ホストPC側で実行コマンド
$ roscore
$ rosrun image_view image_view image:=/camera/color/image_raw
>>>
#### [ INFO] [1642672378.299242713]: Initializing nodelet with 8 worker threads.
#### [ INFO] [1642672378.354235859]: Using transport "raw"
$ python3 color_vel.py
>>>
#### [INFO] [1642672394.648472]: blue=0, red=0
#### [INFO] [1642672395.452979]: blue=3741, red=288
#### [INFO] [1642672396.209016]: blue=6081, red=717
#### [INFO] [1642672396.993956]: blue=6182, red=708
#### [INFO] [1642672397.801500]: blue=6095, red=689
#### [INFO] [1642672398.609366]: blue=6338, red=716
〜〜〜
〜〜〜


Jetson側Lidarで読み込んでる画像の青と赤の領域の値が、手前のホストPCに送られてくるのがわかる。

f:id:trafalbad:20220120191237j:plain

なんとかJetsonでLidarのセットアップと、Jetson側のLidarからホストPC側へのデータ送信(Topic通信)にも成功した。

参考

how to install (installation_jetson.md)
GitHub:realsense-ros

PCからJetson NanoをROSで遠隔操作とsshログインするまで

ホストPCから大体ハードウェアのJetson NanoをROSの遠隔操作とsshでログインまでの備忘録

目次
1 Jetson Nanoのセットアップ
2.Jetson NanoにROSのinstall
3. Realsense Lidarを「realsense-viewer」写すためLibrealsenseインストール
4.ホストPCからJetson NanoをROSで遠隔操作
5. ホストPCからJetsonへsshでログイン

1 Jetson Nanoのセットアップ

まず以前の記事と同じようにJetsonのセットアップから。

今回はJetPack==6.0のMicro SDカードに焼いた。

trafalbad.hatenadiary.jp


# SDカードは/dev/disk4パーティションの容量確保
$ sudo diskutil partitionDisk /dev/disk4 1 GPT "Free Space" "%noformat%" 100% 
>>>>
### Started partitioning on disk4
### Unmounting disk
### Creating the partition map
### Waiting for partitions to activate
### Finished partitioning on disk4

Etcherで書き込み。



2.Jetson NanoにROSのinstall

Jetson Nano(JetPack:6.0)環境
ubuntu 18.04.5 LTS
CMake 3.10.2
Python 3.6.9
Opencv 4.1.1

Jetson起動してubuntuをinstallする設定をした後、18.04なのでMerodicをinstall

まず「Software」から「Firefox ウェブブラウザ」をinstall。

# Jetson起動してupdate
sudo apt update
sudo apt upgrade
sudo apt dist-upgrade
# 必要パッケージのinstall
sudo apt install wget curl git vim tree
sudo apt install -y python3-pip

# ROSのinstall
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

# public keyの登録
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -

# rosと関連パッケージのinstall
sudo apt update
sudo apt install ros-melodic-desktop-full
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
sudo rosdep init
rosdep update

# ワークスペースの作成と登録
mkdir -p catkin_ws/src && cd catkin_ws # (pwd==/home/user/catkin_ws)
catkin_make 
#  ~/.bashrcの「source /opt/ros/melodic/setup.bash」を消す
echo "source /home/user/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

# working spaceの登録
cd ~/catkin_ws/src
catkin_init_workspace
cd ~/catkin_ws
catkin_make

source /home/user/catkin_ws/devel/setup.bash
source ~/.bashrc


# パッケージの作成
cd ~/catkin_ws/src
catkin_create_pkg ros_start rospy roscpp std_msgs
cd ~/catkin_ws
catkin_make
# 作成できてるかcheck
roscd ros_start
#### => ~/catkin_ws/src/ros_start に移動できればOK


3. Realsense Lidarを「realsense-viewer」写すLibrealsenseインストール

realsense-viewer
でRealsense Lidarをjetson上で動かしてみる。

参考:installation_jetson.md

installed at 2022/01/17

# public keyの登録
sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

# サーバーレポジトリのlistの登録
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

#  SDKのinstall
sudo apt-get install librealsense2-utils
sudo apt-get install librealsense2-dev
# 認識してるか確認
ls /dev/video*
### /dev/video0  /dev/video1  /dev/video2  /dev/video3

realsense-viewerでLidarを写してみる。

f:id:trafalbad:20220118091927j:plain




4. ホストPCからJetson NanoをROSで遠隔操作

# ホストPC側
$ hostname -I | cut -d' ' -f1
### 192.168.10.111

# Jetson Nano側
$ hostname -I | cut -d' ' -f1
### 192.168.10.102


環境変数の設定

# PC(ROS_MASTER)側に、ROS_MASTER/ROS_IPを設定
export ROS_MASTER_URI=http://192.168.10.111:11311  # ROS_MASTER側のアドレスを設定
export ROS_IP=192.168.10.111                      # 自分のIPアドレスを設定

# Jetson側、ROS_MASTER/ROS_IPを設定
export ROS_MASTER_URI=http://192.168.10.111:11311
export ROS_IP=192.168.10.102


roscore/turtlesim起動

# PC(ROS_MASTER)側、roscore/turtlesim_nodeを起動
roscore
rosrun turtlesim turtlesim_node

# Jetson側、turtle_teleop_keyを起動
rosrun turtlesim turtle_teleop_key

### Jetson側でturtleを操作できればOK

ホストPCからJetsonをROSで操作できた

f:id:trafalbad:20220117030054j:plain

5. ホストPCからJetsonへsshでログイン

ホストPC側で操作

$ cd ~/.ssh
$ ssh-keygen -t rsa -b 4096  # 秘密鍵/公開鍵ペア生成(鍵の名前を"jetson"にしておく)
$ mv id_rsa jetson && mv id_rsa.pub jetson.pub
# ~/.ssh/configファイルの作成
Host 任意の名称(接続時に使用)
    Hostname 接続先のIPアドレス/ドメイン名
    User ユーザ名
    Port ポート番号
    Identityfile 鍵ファイルのパス
    ServerAliveInterval 秒(自動切断防止設定。後述)

# example
Host jetson
    Hostname 192.168.10.102
    User jetson
    Port 22
    Identityfile ~/.ssh/jetson
    ServerAliveInterval 60

これでホストPCから、jetsonにでリモートログインできた。

ssh jetson 
Jetson pass>>>
>>>

loginできた。次はjetsonからrealsense-Laidarのデータ取得

参考

Jetson Nano/PC間でROS接続する

ubuntu 20.04のROSでRealSense Lidarのセットアップ

ROSでLidarを起動してみるまでのセットアップ手順


環境
Ubuntu 20.04
・ROS noetic


ちなみに製品は「Intel RealSense LiDAR Camera L515」。
Note: Set up at 2022/01/13



目次
1 Intel Realsense SDK インストール
2. realsense-rosのセットアップ
3. ROSでRealsense Lidarを起動
4. Realsense Lidarのrostopic list


1 Intel Realsense SDK インストール

以下の公式インストール手順に従う。

github.com

# 公開鍵を登録
$ sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

# サーバーをレポジトリリストに登録
$ sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

# ライブラリをインストール
$ sudo apt update
$ sudo apt upgrade
$ sudo apt-get install librealsense2-dkms
$ sudo apt-get install librealsense2-utils

# 開発者用ツール、デバッガツールをインストール
$ sudo apt-get install librealsense2-dev
$ sudo apt-get install librealsense2-dbg


試しに起動してみる
$ realsense-viewer

f:id:trafalbad:20220114202848p:plain
3Dでdepth画像まで写ってる。ハイスペック。


ROSのインストール
ROS(noetic)はinstall済み。
trafalbad.hatenadiary.jp




2. realsense-rosのセットアップ

$ cd ~/catkin_ws/src
$ git clone https://github.com/IntelRealSense/realsense-ros.git
$ cd realsense-ros/
#### Note:Switching to '2.3.2' 
#### HEAD is now at f400d68 2.3.2

# ddynamic_reconfigureのinstall
$ sudo apt-get install ros-noetic-ddynamic-reconfigure
# ROSでrealsense2_cameraをinstall
$ sudo apt-get install ros-noetic-realsense2-camera
# catkin make
$ cd ~/catkin_ws
$ catkin_make clean
>>>
〜〜〜
####
#### Running command: "make clean -j8 -l8" in "/home/hagi/catkin_ws/build"
####

$ catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
>>>
〜〜〜
#### [100%] Linking CXX shared library /home/hagi/catkin_ws/devel/lib/librealsense2_camera.so
#### [100%] Built target realsense2_camera
$ catkin_make install
>>>
〜〜〜〜
#### -- Installing: /home/hagi/catkin_ws/install/share/realsense2_camera/rviz/t265.rviz
#### -- Installing: /home/hagi/catkin_ws/install/share/realsense2_camera/nodelet_plugins.xml

3. ROSでRealsense Lidarを起動

ROSでRealtimesense Lidarの起動

# 1つ目のターミナルで
$ roslaunch realsense2_camera rs_camera.launch 
# 2つ目のターミナルで
$ rosrun rqt_image_view rqt_image_view 


f:id:trafalbad:20220114202738p:plain


別の方法で表示

# 1つ目のターミナルで
$ roslaunch realsense2_camera rs_camera.launch 
# 2つ目のターミナルで (rosrun image_view image_view  image:={ros topic path})
$ rosrun image_view image_view  image:=/camera/color/image_raw

f:id:trafalbad:20220115175417p:plain


4. Realsense Lidarのrostopic list

rostopicコマンドで表示できるリスト一覧
rosrun image_view image_view image:={ros topic path}

$ rostopic list
>>>
#### /camera/color/camera_info
#### /camera/color/image_raw
#### /camera/color/image_raw/compressed
#### /camera/color/image_raw/compressed/parameter_descriptions
#### /camera/color/image_raw/compressed/parameter_updates
#### /camera/color/image_raw/compressedDepth
#### /camera/color/image_raw/compressedDepth/parameter_descriptions
#### /camera/color/image_raw/compressedDepth/parameter_updates
#### /camera/color/image_raw/theora
#### /camera/color/image_raw/theora/parameter_descriptions
#### /camera/color/image_raw/theora/parameter_updates
#### /camera/color/metadata
#### /camera/depth/camera_info
#### /camera/depth/image_rect_raw
#### /camera/depth/image_rect_raw/compressed
#### /camera/depth/image_rect_raw/compressed/parameter_descriptions
#### /camera/depth/image_rect_raw/compressed/parameter_updates
#### /camera/depth/image_rect_raw/compressedDepth
#### /camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
#### /camera/depth/image_rect_raw/compressedDepth/parameter_updates
#### /camera/depth/image_rect_raw/theora
#### /camera/depth/image_rect_raw/theora/parameter_descriptions
#### /camera/depth/image_rect_raw/theora/parameter_updates
#### /camera/depth/metadata
#### /camera/extrinsics/depth_to_color
#### /camera/l500_depth_sensor/parameter_descriptions
#### /camera/l500_depth_sensor/parameter_updates
#### /camera/motion_module/parameter_descriptions
#### /camera/motion_module/parameter_updates
#### /camera/realsense2_camera_manager/bond
#### /camera/rgb_camera/parameter_descriptions
#### /camera/rgb_camera/parameter_updates
#### /diagnostics
#### /rosout
#### /rosout_agg
#### /tf
#### /tf_static

参考

ubuntu20.04とROS NoeticでRealSenseを使う手順
realsense-ros(GitHub)
librealsense(Github)
RealSense D435をROSで使う

ROSでc++のコードのコンパイルから実行方法まとめ

本来、実用ではpythonじゃなくてc++を使うことが多いROS。

なので、ROSでのc++のコードをコンパイルして実行する方法をまとめとく



c++スクリプトの用意

今回は待ち状態のServer用pythonスクリプトをcppに書き換えて実行してみる。

server用のpythonスクリプトはこれ
joy_twist.py

import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist

class JoyTwist(object):
    def __init__(self):
        self._joy_sub = rospy.Subscriber('joy', Joy, self.joy_callback, queue_size=1)
        self._twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)

    def joy_callback(self, joy_msg):
        if joy_msg.buttons[0] == 1:
            twist = Twist()
            twist.linear.x = joy_msg.axes[1] * 0.5
            twist.angular.z = joy_msg.axes[0] * 1.0
            self._twist_pub.publish(twist)


if __name__ == '__main__':
    rospy.init_node('joy_twist')
    joy_twist = JoyTwist()
    rospy.spin()


これをc++に書き換えたのがjoy_twist.cpp

joy_twist.cpp

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>

class JoyTwist
{
public:
  JoyTwist()
  {
    ros::NodeHandle node;
    joy_sub_ = node.subscribe("joy", 1, &JoyTwist::joyCallback, this);
    twist_pub_ = node.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  }
  
  void joyCallback(const sensor_msgs::Joy &joy_msg)
  {
    if (joy_msg.buttons[0] == 1)
    {
      geometry_msgs::Twist twist;
      twist.linear.x = joy_msg.axes[1] * 0.5;
      twist.angular.z = joy_msg.axes[0] * 1.0;


      twist_pub_.publish(twist);
    }
  }
private:
  ros::Subscriber joy_sub_;
  ros::Publisher twist_pub_;
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "joy_twist");
  JoyTwist joy_twist;
  ros::spin();
}


ROSでc++を実行してみる

# c++スクリプトの作成
$ roscd ros_start
$ mkdir src
$ vi src/joy_twist.cpp



# コンパイル用にCMakeLists.txtに追記
$ roscd ros_start
$ vi CMakeLists.txt


CMakeLists.txtに以下の2行を追加

add_executable(joy_twist src/joy_twist.cpp)
target_link_libraries(joy_twist
   ${catkin_LIBRARIES}
)
# プログラムで使うためにcatkin_make
cd ~/catkin_ws
catkin_make
>>>
####
#### Running command: "make -j8 -l8" in "/mnt/c/Users/hagi/Downloads/place/catkin_ws/build"
####
#### Scanning dependencies of target joy_twist
#### [  0%] Built target std_msgs_generate_messages_eus
#### [  0%] Built target std_msgs_generate_messages_py
#### [  0%] Built target std_msgs_generate_messages_cpp
#### [  0%] Built target std_msgs_generate_messages_nodejs
#### [  0%] Built target std_msgs_generate_messages_lisp
#### [  0%] Built target _ros_start_generate_messages_check_deps_SetVelocity
#### [ 11%] Building CXX object ros_start/CMakeFiles/joy_twist.dir/src/joy_twist.cpp.o
#### [ 33%] Built target ros_start_generate_messages_cpp
#### [ 33%] Built target ros_start_generate_messages_nodejs
#### [ 55%] Built target ros_start_generate_messages_py
#### [ 77%] Built target ros_start_generate_messages_eus
#### [ 88%] Built target ros_start_generate_messages_lisp
#### [ 88%] Built target ros_start_generate_messages
#### [100%] Linking CXX executable /mnt/c/Users/hagi/Downloads/place/catkin_ws/devel/lib/ros_start/joy_twist
#### [100%] Built target joy_twist

# 実行ファイルができてるか確認
$ ls devel/lib/ros_start
>>>
#### joy_twist

# 1つ目のterminalで
roscore
# 2つ目でc++をROSで実行
$ roscd ros_start
$ rosrun ros_start joy_twist

f:id:trafalbad:20220112225459p:plain

Server用のc++が実行できてるのがわかる。

ROSで独自Service用ファイルを作ってService通信を試す

ROSの通信で頭の隅に置いとく知識

・ROSで行う通信(Topic通信とか)はmsgファイルのデータ型で行う。
・Service通信も拡張子がsrvのファイルのデータ型で行う。


備忘録なので独自設定のservice通信を動かすまでの過程を、淡々とまとめるだけの構成。

目次
1. 基礎的なService通信をやってみる
2. 独自Service用ファイルの作成
3. 独自Serviceの実行


1. 基礎的なService通信をやってみる


Service通信は双方向通信のことで、webサービスでよく使うcurlで投げて結果をクライアントから受け取る通信と同じ。

サービスはROSの通信方法の一つで、双方向通信に使います。ある仕事をお願いするクライアントとそれを処理して返すサーバーからなります。今回、作成するプログラムは速度指令値を送るクライアントと、それをロボットへ伝え(リクエスト)、ロボットから現在の速度を取得しクライアントに返す(レスポンス)サーバーです。
引用元:ROS演習4:双方向通信しよう!(サービス) – demura.net


f:id:trafalbad:20220112213637p:plain


まずは基礎的なService通信をやってみる

Service Serverの作成


roscd ros_start
vi scripts/service_server.py

service_server.py

#!/usr/bin/env python3
import rospy 
from std_srvs.srv import Empty
from std_srvs.srv import EmptyResponse

def handle_service(req):
    rospy.loginfo('called !!')
    return EmptyResponse()

def service_server():
    rospy.init_node('service_server')
    s = rospy.Service('call_me', Empty, handle_service)
    print("Ready to serve")
    rospy.spin()

if __name__=='__main__':
    service_server()


Clientの作成



vi scripts/service_client.py

service_client.py

#!/usr/bin/env python3
import rospy 
from std_srvs.srv import Empty

def call_service():
    rospy.loginfo('waiting service')
    rospy.wait_for_service('call_me')
    try:
        service = rospy.ServiceProxy('call_me', Empty)
        response = service()
    except rospy.ServiceException as e:
        print('Service call failed: %s' % e)

def service_client():
    rospy.init_node('service_client')
    call_service()
    rospy.spin()

if __name__=='__main__':
    service_client()
    call_service()

Service通信の実行


# 1つ目のterminal
roscore 

# 2つ目のterminal
roscd ros_start
rosrun ros_start service_server.py

#### serverが待ちの状態
# 3つ目のterminal
roscd ros_start
rosrun ros_start service_client.py

clientから指令を出して、Service側で受け取った。

f:id:trafalbad:20220112213819p:plain



2. 独自Service用ファイルの作成


今回はシミュレーション上のロボットを独自の設定のService通信で制御する。

設定内容

・外部プログラムから「速度」「角度」を指定して動かす
・指定範囲内の速度・角度だけ受け取って'success'を返す
・指定範囲外は全て'failed'を返す


入力速度、角度(float64)
出力sucessかfailedの文字列(bool)

clientで入力を受け取り、serverで出力を返すように設定したsrvファイルを作ってservice通信を行う。


f:id:trafalbad:20220112213732p:plain



1. SetVelocity.srvの作成


roscd ros_start
mkdir srv
vi srv/SetVelocity.srv


SetVelocity.srv

float64 linear_velocity
float64 angular_velocity
---
bool success

2. package.xmlの書き換え


message_generation
message_runtime
コメントアウトする。

$ roscd ros_start
$ vi package.xml
<build_depend>message_generation</build_depend>
 <!-- Use build_export_depend for packages you need in order to build against this package: -->
 <!--   <build_export_depend>message_generation</build_export_depend> -->
 <!-- Use buildtool_depend for build tool packages: -->
 <!--   <buildtool_depend>catkin</buildtool_depend> -->
 <!-- Use exec_depend for packages you need at runtime: -->
 <exec_depend>message_runtime</exec_depend>


3. CMakeLists.txtの書き換え



vi CMakeLists.txt

以下の3箇所を書き換え

CMakeLists.txt

## Generate services in the 'srv' folder
add_service_files(
  FILES
  SetVelocity.srv #書き換え
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs #追加
)

## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
 roscpp
 rospy
 std_msgs
 message_generation #追加
)
# check
rossrv show ros_start/SetVelocity
>>>
### float64 linear_velocity
### float64 angular_velocity
### ---
### bool success


4. プログラムで使うためにcatkin_make


cd ~/catkin_ws
catkin_make
>>>
### [100%] Built target ros_start_generate_messages_py
### Scanning dependencies of target ros_start_generate_messages
### [100%] Built target ros_start_generate_messages

# 再起動
sudo reboot


これで独自serviceファイルがプログラムで使えるようになった。


3. 独自Serviceの実行

1. serverスクリプトの作成



roscd ros_start/scripts
vi velocity_server.py

velocity_server.py

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from ros_start.srv import SetVelocity
from ros_start.srv import SetVelocityResponse

MAX_LINEAR_VELOCITY = 1.0
MIN_LINEAR_VELOCITY = -1.0
MAX_ANGULAR_VELOCITY = 2.0
MIN_ANGULAR_VELOCITY = -2.0

def velocity_handler(req):
    vel = Twist()
    is_set_success = True
    if req.linear_velocity <= MAX_LINEAR_VELOCITY and (
            req.linear_velocity >= MIN_LINEAR_VELOCITY):
        vel.linear.x = req.linear_velocity
    else:
        is_set_success = False
    if req.angular_velocity <= MAX_ANGULAR_VELOCITY and (
            req.angular_velocity >= MIN_ANGULAR_VELOCITY):
        vel.angular.z = req.angular_velocity
    else:
        is_set_success = False
    print(vel)
    if is_set_success:
        pub.publish(vel)
    return SetVelocityResponse(success=is_set_success)

if __name__ == '__main__':
    rospy.init_node('velocity_server')
    pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10)
    service_server = rospy.Service('set_velocity', SetVelocity, velocity_handler)
    rospy.spin()


2. clientスクリプトの作成



vi velocity_client.py
velocity_client.py

#!/usr/bin/env python3
import rospy
from ros_start.srv import SetVelocity
import sys

if __name__ == '__main__':
    rospy.init_node('velocity_client')
    set_velocity = rospy.ServiceProxy('set_velocity', SetVelocity)
    linear_vel = float(sys.argv[1])
    angular_vel = float(sys.argv[2])
    response = set_velocity(linear_vel, angular_vel)
    if response.success:
        rospy.loginfo('set [%f, %f] success' % (linear_vel, angular_vel))
    else:
        rospy.logerr('set [%f, %f] failed' % (linear_vel, angular_vel))
roscd ros_start
sudo chmod 755 scripts/*

3. service通信を実行



# 1つ目のteminalで
roscore

# 2つ目のteminalでServer(velocity_server.py)の実行
roscd ros_start
rosrun ros_start velocity_server.py

# 3つ目のteminalでclient(velocity_client.py)の実行

# パラメータ 2.0 3.0で1回目の送信 
rosrun ros_start velocity_client.py 2.0 3.0
### [ERROR] [1641964505.376901]: set [2.000000, 3.000000] failed

指定範囲外なので失敗。failedがserviceから返ってきた

# パラメータ 0.5 0.0で2回目の送信

rosrun ros_start velocity_client.py 0.5 0.0
### [INFO] [1641964584.662343]: set [0.500000, 0.000000] success

指定範囲内なので成功。=> successがserviceから返ってきた

f:id:trafalbad:20220112215855p:plain

Serviceの基本と独自Serviceを作って動かすまででした。
これを実際のロボットを遠隔で制御操作するのが目的。

ROSの基本プログラムのTopic通信(非同期通信)を動かす

今回はROSの基本のPublisherとSubscriberを使ったROSの基本プログラムtopic通信(非同期通信)をうごかしたので、その備忘録。

仕組みとしてはこんな感じ

・Topicデータを書き込む場所(非同期通信のメイン)
・Servicepublisher側でsubscriberの結果を受け取る機能。publisherと一緒に呼び出す
・Publisherデータを書き込むノードとスクリプト
・Subscriberデータを読み込むノードとスクリプト

f:id:trafalbad:20220111154013p:plain


目次
1.ROSの「turtlesim」を試す
2. ROSの基本プラグラムでtopic通信の実行
3. roslaunchで一斉に(ターミナル1つで)プログラムの実行
4. rostopicでroslaunchのログの確認
追記:エラー対策

1.ROSの「turtlesim」を試す

まずROSでの基本「turtlesim」を試してみる

# 1つ目のターミナルで
$ roscore
# 2つ目のターミナルでnodeの起動
$ rosrun turtlesim turtlesim_node
# 3つ目のターミナルで制御するnodeを動かす
$ rosrun turtlesim turtle_teleop_key

#### カメが表示されてカーソルを押せば動く

# 4つ目のターミナルで動作中の対応関係を表示
$ rqt_graph


f:id:trafalbad:20220113201427p:plain

簡単にROSを試してみた。

2. ROSの基本プラグラムでtopic通信の実行

Publisher(talker.py)の作成



cd src/ros_start # roscd ros_start
# python scripts用フォルダを作る
mkdir scripts && cd scripts
# Publisher(talker.py)の作成
vi talker.py


talker.py

#!/usr/bin/env python3
import rospy
from std_msgs.msg import String

# create node named "talker"
rospy.init_node("talker")
# create Publisher and "chatter" is Topic name
pub = rospy.Publisher("chatter", String, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
    hello_str = String()
    hello_str.data = "hello world %s" % rospy.get_time()
    pub.publish(hello_str)
    rate.sleep()


Subscriber(listener.py)の作成



roscd ros_start
# Subscriber(listener.py)の作成
vi listener.py

listener.py

#!/usr/bin/env python3
import rospy
from std_msgs.msg import String

def callback(message):
    rospy.loginfo("I heard %s", message.data)

# create node named "listener"
rospy.init_node("listener")
# create Subscriber and "chatter" is Topic name
sub = rospy.Subscriber("chatter", String, callback)
rospy.spin()


ターミナルを3個使ってプログラムの実行



chmod 755 talker.py listener.py

#1こ目のteminalで
roscore

# 2こめで

roscd ros_start (pwd==~/catkin_ws/src/ros_start)
rosrun ros_start talker.py

#3こめで
rosrun ros_start listener.py
>>>

#### [INFO] [1641830803.522044]: I heard hello world 1641830803.5217884
#### [INFO] [1641830803.622811]: I heard hello world 1641830803.6220603
#### [INFO] [1641830803.722807]: I heard hello world 1641830803.7220128
#### ~~~~~


動いてることが確認できた。


3. roslaunchで一斉に(ターミナル1つで)プログラムの実行

terminalを1つで
・roscore
・Publisher(talker.py)
・Subscriber(listener.py)

を一斉に実行する。

# 一斉に実行するためのスクリプトの作成
roscd ros_start
mkdir launch
vi launch/chat.launch

chat.launch

<launch>
    <node pkg="ros_start" name="talker" type="talker.py"/>
    <node pkg="ros_start" name="listener" type="listener.py"/>
</launch>
roscd ros_start
roslaunch ros_start chat.launch
>>>
#### ROS_MASTER_URI=http://localhost:11311
#### process[talker-1]: started with pid [656]
#### process[listener-2]: started with pid [657]


logは出力されないけど、エラーが出ないので、ちゃんと動いてる。
logは「lostopic」で確認できる。



4. rostopicでroslaunchのログの確認

もう1つterminalを立ち上げる。

# 出力リストを表示
rostopic list
>>>
#### /chatter
#### /rosout
#### /rosout_agg
# logを表示
rostopic echo /chatter

#### ---
#### data: "hello world 1641831594.0200205"
#### ---
#### data: "hello world 1641831594.1205244"
#### ---
#### data: "hello world 1641831594.2202258"
#### ---
#### data: "hello world 1641831594.3208992"
#### 〜〜


ちゃんと動いてることが確認できた。
ROSでのTopic通信(非同期通信)でした。


追記:エラー対策

rosrunの実行時に
/usr/bin/env: ‘python3\r’: No such file or directory
のエラーが出る場合は改行コードを『Lu』に統一してやる必要があるので

sudo apt install nkf
nkf -Lu --overwrite [ファイル名]

で解決できる。
参考サイト改行コードを変換する

ROSのインストールと基礎動作確認まで

ロボットを動かすミドルウェアrosを使うために、rosのinstallと簡単な初期動作の備忘録。
rosのバージョンはNoetic。






ROSのバージョン表
ディストリビューション Ubuntuのバージョン サポート期限
Indigo
14.04
2019年5月
Kinetic
14.04
2021年5月
Melodic
18.04
2023年5月
Noetic
20.04
2025年5月

Windows 11をOSごとubuntu20.04に入れ替えた。
(ubuntu 18.04だとwifiを認識しなかったり、マウスパッドが反応しないとかのエラーが出たので20.04がおすすめ)
(20.04だとcmakeとpython3はすでに入ってる。)


環境
OS Ubuntu : 20.04.3 LTS
CMake :3.16.3
Python :3.8.10
opencv : 4.2.0(rosのinstall時に入る)

f:id:trafalbad:20220110022226j:plain

目次
1. ROSのインストール
2. ROSのインストール確認
3. ROSのワーキングスペースの作成
4. Opencvシンボリックリンクの作成
5. ROSパッケージの作成
6. 簡単なROSの基本動作

1. ROSのインストール

# 必須パッケージのinstall
sudo apt install curl git wget vim tree
sudo apt install -y python3-pip

****注意 conda(anaconda3)はinstallしたらrosのinstall時にエラーが起きた

# パスの登録
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
# public keyの登録
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -

# rosと関連パッケージのinstall
sudo apt update 
# ros のinstallと同時にopencvも入る(/usr/include/opencv4)
sudo apt install ros-noetic-desktop-full
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

# rosdepの初期化(エラーが出たら$ sudo rm /etc/ros/rosdep/sources.list.d/20-default.list)
sudo rosdep init
rosdep update

# ワークスペースの作成と登録
mkdir -p catkin_ws/src && cd catkin_ws
catkin_make # (If It's ok : pwd ==/mnt/c/Users/Downloads/place/catkin_ws)

# "source /opt/ros/noetic/setup.bash" と書き換え
echo "source /mnt/c/Users/Downloads/place/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc


2. ROSのインストール確認

インストールできてるかチェック

# 1つ目
$ echo $ROS_PACKAGE_PATH
>>>>
/mnt/c/Users/Downloads/place/catkin_ws/src:/opt/ros/noetic/share
と表示されればOK
# 2つ目
$ roscore
#色々表示されればOK

Noeticのインストール用script(install.sh)

# !/bin/sh
# resister path to install ros & get public key to download
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
# install ros(melodic)
sudo apt update
sudo apt install ros-melodic-desktop-full
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

# sudo rm /etc/ros/rosdep/sources.list.d/20-default.list
sudo rosdep init
rosdep update

# create work space
mkdir -p catkin_ws/src && cd catkin_ws
catkin_make
# register worksopace
echo "source /mnt/c/Users/Downloads/place/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
# To check
echo $ROS_PACKAGE_PATH 


ROSのuninstall(全部削除)

# uninstall ros completely 
$ sudo apt-get purge ros-*
$ sudo apt-get autoremove

3. ROSのワーキングスペースの作成

# working spaceを作成してbuild
mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src
catkin_init_workspace
>>>
#### Creating symlink "/mnt/c/Users/Downloads/place/catkin_ws/src/CMakeLists.txt" pointing to "/opt/ros/noetic/share/catkin/cmake/toplevel.cmake"


# catkin makeでbuildしてROSのwaking spaceを作成
cd ~/catkin_ws
catkin_make
>>>
####
#### Running command: "make -j8 -l8" in "/mnt/c/Users/hagi/Downloads/place/catkin_ws/build"
####


# ~/.bashrcの /opt/ros/noetic/setup.bashをけして、書き換え
source /mnt/c/Users/Downloads/place/catkin_ws/devel/setup.bash
source ~/.bashrc


$ echo $ROS_PACKAGE_PATH

/mnt/c/Users/Downloads/place/catkin_ws/src:/opt/ros/noetic/share
と表示されれば、ROSの実行場所が作成できた。


4. Opencvシンボリックリンクの作成

opencvのパスが/usr/include/opencv4/opencv2になってるのでシンボリックリンクを作る。

sudo ln -s /usr/include/opencv4/ /usr/include/opencv

5. ROSパッケージの作成

# 基本パッケージの作成(catkin make でbuild)
cd ~/catkin_ws/src
catkin_create_pkg ros_start rospy roscpp std_msgs
>>>>
#### Created file ros_start/package.xml
#### Created file ros_start/CMakeLists.txt
#### Created folder ros_start/include/ros_start
#### Created folder ros_start/src
#### Successfully created files in /mnt/c/Users/Downloads/place/catkin_ws/src/ros_start. Please adjust the values in package.xml.



# catkin make
cd ~/catkin_ws
catkin_make
>>>
####
#### Running command: "make -j8 -l8" in "/mnt/c/Users/Downloads/place/catkin_ws/build"
####

# 作成できてるかcheck
roscd ros_start
#### => ~/catkin_ws/src/ros_start に移動できればOK


6. 簡単なROSの基本動作

2. ROSでHello World



1. ターミナルを2つ開く

2. 一つ目のターミナルで
roscore

3. 2つ目のターミナルでpythonスクリプト実行

# hello.py
import rospy
rospy.init_node('hello') # ノード初期化
rospy.loginfo('Hello World')
rospy.spin()
$ python3 hello.py
>>>
[INFO][WallTime: 1456677777.322475] Hello World

ROSでHello Worldできた。
GUIも使えるし、VPNにもつなぎやすいので、Windowsでwslより、ubuntuでOSを入れ替える方がいい。


参考サイト

『ROS Noetic』のインストール