初めてのロボットアーム作りに挑戦！5-1 PFRLでまずは簡単な動作を学習(前半)
出田守です。
アイアンマンを観て、ロボットアームが欲しくなりました。
勉強しながら、初めてのロボットアーム作りに挑戦します。
今回の目標
- PFRLを使って簡単な動作を学習
今回はロボットアーム君に簡単な動作をPFRLを使って学習させたいと思います。
長くなったので前半と後半に分けています。
環境
- Windows 10 (PFRL, ROS) + GeForce GTX 960
- CUDA Toolkit 10.1 update2
- Raspberry pi 4, Raspberry pi camera v2 (ROS)
- Python 3.8.5, Python 2.7.15
- PyTorch 1.7.0+cu101
今回挑戦するタスク
単純にロボットアーム君がボタンを押すという動作を学習させようと思います。
まず全体のブロックチャートを載せます。
できればもっとROSを活用したいですが、まずはPFRLに慣れます。
そして今回のタスクを追加した図を載せます。
スイッチのON/OFF状態をRaspberry Pi 4で受け取り、テキストファイルに記録しておきます。PFRLの報酬計算の際にテキストファイルからスイッチの状態を受け取ります。
スイッチが正しく押された場合は報酬を受け取るという流れです。
RPi.GPIOライブラリを使えるようにする
Ubuntu 18.04をインストールしたRaspberry Pi 4上でGPIOを読み書きをできるようにします。私はよく使われる「RPi.GPIO」を使用することにしました。
インストール自体は簡単で、
sudo apt update sudo apt upgrade sudo apt install python-pip python-dev pip install RPi.GPIO
だけです。
しかし、ライブラリをインポートして使おうとすると、RuntimeError: Not running on a RPi!というエラーが発生するので、ubuntuユーザがgpioを触る権限を以下のように与えます。
ls -l /dev/gpiomem sudo chown root:$USER /dev/gpiomem sudo chmod g+rw /dev/gpiomem
あるいは以下でもいけるかもしれません。
sudo adduser username gpio
エラー時の参考URL
- https://github.com/gpiozero/gpiozero/issues/837
- https://github.com/adafruit/Adafruit_Blinka/issues/150
GPIO17を使って、まずはOUTPUTできるかテストします。テストコードは以下です。
import RPi.GPIO as GPIO GPIO.setmode(GPIO.BCM) GPIO.setup(17, GPIO.OUT) GPIO.output(17, 1) try: while True: pass except: pass finally: GPIO.cleanup(17)
テスターなどでGPIO17の電圧が3.3Vになっていることを確認します。Ctrl-cでストップします。
次にINPUTできるかもテストします。
import RPi.GPIO as GPIO import time GPIO.setmode(GPIO.BCM) GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP) try: while True: print(GPIO.input(17)) time.sleep(1) except: pass finally: GPIO.cleanup(17)
ターミナル上に「1」が表示されるはずなので、GPIO17にGroundを接続してあげると「0」に切り替わることを確認します。
以上でGPIOを使えるようになりました。
ROSのPublisherとSubscriberを作成
この時点で気付いてしまったのですが、ROSはPython2をPFRLはPython3を使うので、実行バージョンが合っていないことになります。ROS2をインストールするべきだったかもしれません・・・。
とりあえず、今回はROSからパブリッシュされる画像と ボタンの状態(On/Off)をサブスクライブし保存して、PFRLから都度読み込むという形でやろうと思います。
cv_cameraですでにpublisherがあるので、残りは、画像のsubscliberとボタン状態のpublisherとsubscliberを作ります。
ワークスペース作成
まずはワークスペースを作成します。
mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src catkin_init_workspace cd ../ catkin_make source devel/setup.bash
windowsでは、
mkdir %HOME%\catkin_ws\src cd %HOME%\catkin_ws\src catkin_init_workspace cd ..\ catkin_make devel\setup.bat
画像のsubscliber
- パッケージを作成します。
cd catkin_ws\src catkin_create_pkg save_image std_msgs roscpp
- package.xmlとCMakeLists.txtを編集
package.xmlは以下のように編集しました。<?xml version="1.0"?> <package format="2"> <name>save_image</name> <version>0.1.0</version> <description>This package stores the image data published by cv_camera.</description> <!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <maintainer email="jobs4beggieboo@gmail.com">biggieboo</maintainer> <!-- One license tag required, multiple allowed, one license per tag --> <!-- Commonly used license strings: --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> <license>MIT</license> <!-- Url tags are optional, but multiple are allowed, one per tag --> <!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Example: --> <!-- <url type="website">http://wiki.ros.org/save_image</url> --> <url type="repository">https://github.com/BiggieBoo18/mr_robotarm</url> <url type="website">https://detamamoru.hateblo.jp</url> <!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Authors do not have to be maintainers, but could be --> <!-- Example: --> <!-- <author email="jane.doe@example.com">Jane Doe</author> --> <author email="jobs4beggieboo@gmail.com">biggieboo</author> <!-- The *depend tags are used to specify dependencies --> <!-- Dependencies can be catkin packages or system dependencies --> <!-- Examples: --> <!-- Use depend as a shortcut for packages that are both build and exec dependencies --> <!-- <depend>roscpp</depend> --> <!-- Note that this is equivalent to the following: --> <!-- <build_depend>roscpp</build_depend> --> <!-- <exec_depend>roscpp</exec_depend> --> <!-- Use build_depend for packages you need at compile time: --> <!-- <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> --> <!-- Use test_depend for packages you need only for testing: --> <!-- <test_depend>gtest</test_depend> --> <!-- Use doc_depend for packages you need only for building documentation: --> <!-- <doc_depend>doxygen</doc_depend> --> <buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>std_msgs</build_depend> <build_depend>cv_camera</build_depend> <build_export_depend>roscpp</build_export_depend> <build_export_depend>std_msgs</build_export_depend> <build_export_depend>cv_camera</build_export_depend> <exec_depend>roscpp</exec_depend> <exec_depend>std_msgs</exec_depend> <exec_depend>cv_camera</exec_depend> <!-- The export tag contains other, unspecified, tags --> <export> <!-- Other tools can request additional information be placed here --> </export> </package>
CMakeLists.txtは以下のように編集しました。
cmake_minimum_required(VERSION 3.0.2) project(save_image) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp std_msgs cv_camera ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## ################################################ ## To declare and build messages, services or actions from within this ## package, follow these steps: ## * Let MSG_DEP_SET be the set of packages whose message types you use in ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). ## * In the file package.xml: ## * add a build_depend tag for "message_generation" ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in ## but can be declared for certainty nonetheless: ## * add a exec_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to ## find_package(catkin REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to ## catkin_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # ) ## Generate actions in the 'action' folder # add_action_files( # FILES # Action1.action # Action2.action # ) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs # ) ################################################ ## Declare ROS dynamic reconfigure parameters ## ################################################ ## To declare and build dynamic reconfigure parameters within this ## package, follow these steps: ## * In the file package.xml: ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" ## * In this file (CMakeLists.txt): ## * add "dynamic_reconfigure" to ## find_package(catkin REQUIRED COMPONENTS ...) ## * uncomment the "generate_dynamic_reconfigure_options" section below ## and list every .cfg file to be processed ## Generate dynamic reconfigure parameters in the 'cfg' folder # generate_dynamic_reconfigure_options( # cfg/DynReconf1.cfg # cfg/DynReconf2.cfg # ) ################################### ## catkin specific configuration ## ################################### ## The catkin_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES save_image # CATKIN_DEPENDS roscpp std_msgs # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( # include ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library #add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/save_image.cpp #) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME}_node src/save_image_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ) ############# ## Install ## ############# # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # catkin_install_python(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html # install(TARGETS ${PROJECT_NAME}_node # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html # install(TARGETS ${PROJECT_NAME} # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} # ) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES # # myfile1 # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_save_image.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) - 依存パッケージのインストール
cv_cameraパッケージをインストールします。
まずはsave_image.rosinstallというファイルを作成して、以下のように記述します。- git: {local-name: cv_camera, uri: https://github.com/OTL/cv_camera.git, version: master}vcsを使ってインポートします。
cd catkin_ws vcs import src < src\save_image\save_image.rosinstall rosdep install -i --from-paths src\cv_camera
参考
- 画像のsubscriberノードの作成
catkin_ws/src/save_image/src/save_image_node.cppというファイルで画像保存用のノードを作成します。
コードはcv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImagesを参考に、以下のようにしました。#include <ros/ros.h> #include <image_transport/image_transport.h> #include <cv_bridge/cv_bridge.h> #include <sensor_msgs/image_encodings.h> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/highgui/highgui.hpp> static const std::string OPENCV_WINDOW = "Image window"; class ImageConverter { ros::NodeHandle nh_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; image_transport::Publisher image_pub_; public: ImageConverter() : it_(nh_) { // Subscrive to input video feed and publish output video feed image_sub_ = it_.subscribe("/cv_camera/image_raw", 1, &ImageConverter::imageCb, this); // image_pub_ = it_.advertise("/image_converter/output_video", 1); // cv::namedWindow(OPENCV_WINDOW); } ~ImageConverter() { // cv::destroyWindow(OPENCV_WINDOW); } void imageCb(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } // Draw an example circle on the video stream // if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60) // cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0)); // Save image cv::imwrite("camera_image.jpg", cv_ptr->image); // Update GUI Window // cv::imshow(OPENCV_WINDOW, cv_ptr->image); // cv::waitKey(3); // Output modified video stream // image_pub_.publish(cv_ptr->toImageMsg()); } }; int main(int argc, char** argv) { ros::init(argc, argv, "save_image_node"); ImageConverter ic; ros::spin(); return 0; }
catkin_wsフォルダでビルドします。
catkin_make
ビルドが完了したら、以下のように実行します。
rosrun save_image save_image_node
実行すると、catkin_wsフォルダにcamera_image.jpgというファイルが作成されて、カメラ画像が保存されます。
文量が多く、はてなブログがとてつもなく重くなったので、一旦ここまでとします。
- 画像のsubscriberノードの作成
