前回は、とりあえずORB-SLAM3をROS2に搭載しましたが、搭載しただけでは情報を使う術はありません。ノード終了時に軌道をファイル保存していますが、これで、リアルタイム自律制御は不可能だと思います。そこで、今回は情報を参照できる様に前回作成したノードを再設計します。
とりあえず、自律制御とシミュレーションに必要そうな下記のメッセージ、サービスを加えます。
ORB_SLAM3の搭載情報もないくらいなので、当然、このような情報もなく、やっている方もいなさそうなので、ど素人チャレンジします。
- ポイントクラウド配信（パブリッシャー）
ORB_SLAM3で取得した点群を配信します。
subscriptionした側で、目標物の検知や軌道を決定するための情報として配信します。 - 累積ポイントクラウド配信（パブリッシャー）
リセットされてからのポイントクラウドを蓄積して配信します。
主にはデバッグ、性能評価目的ですが、蓄積することで通過した箇所を地図化できるかな？という感じで搭載してみました。 - 位置、姿勢(xyz座標、クォータニオン)配信（パブリッシャー）
自身の位置、姿勢を認識するための情報を配信します。 - /TFの配信（パブリッシャー）
主には、rvitz2で姿勢や位置を再現するための情報です。
位置、姿勢情報としては、4.と同じですが、ロボット制御対象のフレーム情報を付加しています。
rvitz2用のurdf(ロボットのメカ定義みたいな情報)を作成していないので、適当です。
もし、urdfを作ることがあり、困った時には見直す予定でいます。 - マップのリセット（サービス）
SLAM開始時に呼び出して、蓄積情報をリセットします。
SLAM内部に抱えている地図、座標、累積クラウドポイントをクリアします。
今回はパブリッシャーで配信するデータについては、標準フォーマットを採用しました。
理由は、標準フォーマットとしておくことで、SLAMのアルゴ変更時の影響を最小限度に抑えることができることと、標準的なデータ表示ツールが使用可能であるからです。
今回のポイントクラウド、位置、姿勢、/TFはすべてrvitz2(ROS2標準ツール）で表示可能です。
当然、ネットワークを介しても表示できますので便利だと思います。
■ ポイントクラウド配信
検知された特徴点の空間位置を、sensor_msg/msg/PointCloud2形式に変換して配信します。
検知範囲は、カメラの画角範囲内に限られます。
ROS2標準ツール：rviz2では、下記のオレンジン点線部のように空間距離が表現されます。
（その他の図形は後ほど説明します）
この情報を「picar_map_mono/map_points」で30fpsで配信します。(左記は配信チャンネル名称）
自律制御ソフトは、この情報をsubscriptionすることで、進路方向の障害物、目標位置までの距離を知ることが可能になります。
■ 累積ポイントクラウド配信
過去に検知した特徴点の空間位置も含めて、sensor_msg/msg/PointCloud2形式に変換して配信します。
下図は、適当に動かして取得した情報ですが、実際にカメラで見えていない部分の情報も配信します。
大量の情報を蓄積することになるので、デバッグ用途でのみ使用します。
「picar_map_mono/global_map_points」で30fpsで配信します。
■ 位置、姿勢(xyz座標、クォータニオン)配信
現在の機体の位置、姿勢をgeometry_msgs/msg/PoseStamped形式で配信します。
rvitzでは、下図のオレンジで囲った矢印で位置と姿勢が表現されます。
「/picar_map_mono/pose」で30fpsで配信します。
■ /TFの配信
カメラフレームの位置、姿勢を/TFで表します。
/TFはシミュレーション等で使用するために用意されているデータで、ROS2であらかじめ用意されているIFで位置、姿勢情報を変換して配信します。今回は、まだロボットの関節、フレーム等を定義していませんので、下記のように赤、青、緑で姿勢と位置が表現されます。
実際シミュレートするロボットを定義すると、下記のような形状に同期して動作することになります。
今回は、そこまで手が回っていません。
■マップのリセット
ORB_SLAM3は内部にマップを保持していることと、今回はデバッグ用にマップ生成を行うのでロボット起動タイミング以外でもリセットできるようにしておきます。
「/picar_map_mono/reset_map」にリセットが要求されると、ORB_SLAM３、デバッグ用マップをリセットするようにしてあります。
■ ORB_SLAM3ノードの動作確認
上記までを実装してから軽く動作確認を行いました。
車を手で持って動かしながら、ORB_SLAM3のデバッグ画面とriviz2上のデバッグ情報と見比べてみました。有線LANで動作確認している理由は、ROS2の分散コンピューティングを使っているためです。
今回は、カメラ画像をRaspberry PI上のv4l2_cameraノードで配信し、SLAMはPC上で実行しました。Rasyberry PI3のWiFiでは帯域的に厳しかったのと、ORB_SLAM3のデバッグ画面を同時に見たかったので、有線LAN接続しました。それでも、所々でフレームが飛んでいます。
分散コンピューティングに少し期待をしていたのですが、Raspberry PI3では、これも少々きつそうな感じします。
■ Raspberry PI3上でのORB_SLAM3ノード動作確認結果
分散コンピューティングでは、フレームがたまに飛んでしまうようなので、Raspberry PI3上でORB_SLAM3を動作させてみました。
分散コンピューティングでは、ORB_SLAM3は動作中にデバッグ用に検知対象物やマップを表示するようにしていましたが、さすがにRaspberry PI3上でデバッグ表示は厳しいので、デバッグ表示をすべてdisableにしての実験です。rvitz2でのマップ表示を頼りに操作してみました。
こちらの構成では、少し横方向に大きめに動かすと、フレームを見失ってしまうようことがあるようです。使えなくはないでしょうが、頻繁に位置をロストしているようだと、走行中の使用は厳しいかもしれません。
ORB_SLAM3ノードのCPU負荷をみると、カメラを動かさない時で大体140%前後。
カメラを動かすと180%前後まで増大しています。ORB_SLAM3内部はいくつかのタスクで構成されていますので、負荷的にはギリ大丈夫なように思えます。（少しLatencyがある程度で済むかな？）
ですが、メモリは、常に２GB以上を消費しています。
Raspberry PI3の物理メモリでは、メモリがかなりキツイ状況のように見えます。
■ ORB_SLAM(単眼）ノードのソースコード
下記に試したコードを添付します。ORB_SLAM3の搭載は結構大変でした。
単眼SLAMが必要な方は限られるかもしれないですし、自身の備忘録としても記載しておきます。
〇 ORB_SLAM3ノードのコード
黄色：位置、姿勢および/TFの配信処理
青色：ポイントクラウドおよび累積ポイントクラウド配信
赤色：マップのリセット
/*
* this contains the ORB_SLAM3 node.
*/
#include <opencv2/core/core.hpp>
#include "rclcpp/rclcpp.hpp"
#include "cv_bridge/cv_bridge.h"
#include "tf2/convert.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include "System.h"
#include "Map.h"
#include "Converter.h"
/*
* here's the definition of messages.
*/
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "picar_interfaces/srv/reset_map.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
/*
* here's the configuration of this file.
*/
#define CNF_VOCAVULARY_PATH "orb_slam3/Vocabulary/ORBvoc.txt"
#define CNF_SETTING_PATH "orb_slam3/Setting/TUM.yaml"
#define CNF_NUM_CHAN 3
#define CNF_MIN_OBSERVATION 1
using std::placeholders::_1;
using std::placeholders::_2;
using Image = sensor_msgs::msg::Image;
class PicarMapMonoNode : public rclcpp::Node
{
public:
PicarMapMonoNode(ORB_SLAM3::System *pslam) : Node("picar_map_mono")
{
slam_ = pslam;
/*
* this gets the parameter.
*/
this->declare_parameter("/pointclould_frame_id", "camera_link");
map_frame_id_param_ =
this->get_parameter("/pointclould_frame_id").as_string();
this->declare_parameter("/base_frame_id", "base_link");
base_frame_id_param_ =
this->get_parameter("/base_frame_id").as_string();
this->declare_parameter("/camera_frame_id", "camera_link");
camera_frame_id_param_ =
this->get_parameter("/camera_frame_id").as_string();
this->declare_parameter("/map_mode", 0);
map_mode_ = this->get_parameter("/map_mode").as_int();
/*
* this initializes tf2 listener and tf2 broadcaster.
*/
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>(*this);
/*
* this creates server.
*/
reset_server_ =
this->create_service<picar_interfaces::srv::ResetMap>(
"picar_map_mono/reset_map",
std::bind(&PicarMapMonoNode::resetMap, this, _1, _2));
/*
* this creates subscriber and publisher.
*/
img_subscriber_ = this->create_subscription<Image>(
"/image_raw", 10,
std::bind(&PicarMapMonoNode::trackImage, this, _1));
lmp_publisher_ =
this->create_publisher<sensor_msgs::msg::PointCloud2>(
"picar_map_mono/map_points", 10);
if (map_mode_ != 0) {
mp_publisher_ =
this->create_publisher<sensor_msgs::msg::PointCloud2>(
"picar_map_mono/global_map_points", 10);
}
pose_publisher_ =
this->create_publisher<geometry_msgs::msg::PoseStamped >(
"picar_map_mono/pose", 10);
RCLCPP_INFO(this->get_logger(), "monocular slam has been started.");
}
~PicarMapMonoNode()
{
slam_->Shutdown();
}
private:
ORB_SLAM3::System *slam_; /* ORB_SLAM3 system. */
ORB_SLAM3::Map map_; /* world wide map. */
cv_bridge::CvImagePtr cvimg_; /* is cv2 image. */
/* here's the parameters(of rvitz2). */
std::string map_frame_id_param_;
std::string base_frame_id_param_;
std::string camera_frame_id_param_;
int map_mode_;
/* here's the serives. */
rclcpp::Service<picar_interfaces::srv::ResetMap>::SharedPtr reset_server_;
/* here's the subscriptions. */
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_subscriber_;
/* here's the publishers. */
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr lmp_publisher_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr mp_publisher_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
/* here's the xform listener. */
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
/* here's the tf2 broad caster .*/
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
/*
* purpose : entry to reset map.
*/
void resetMap(
const picar_interfaces::srv::ResetMap::Request::SharedPtr request,
const picar_interfaces::srv::ResetMap::Response::SharedPtr response
)
{
slam_->Reset();
if (map_mode_ != 0) {
map_.clear();
}
response->result = 0;
RCLCPP_INFO(this->get_logger(), "slam map has been reset.");
}
/*
* purpose : entry to track image to build map.
*/
void trackImage(const Image::SharedPtr msg)
{
Sophus::SE3f pose;
/* this converts ros raw image to cv:Mat. to display cloud points. */
try
{
cvimg_ = cv_bridge::toCvCopy(msg);
}
catch (cv_bridge::Exception &e)
{
RCLCPP_ERROR(this->get_logger(), "cv_bridge error %s", e.what());
return;
}
pose = slam_->TrackMonocular(cvimg_->image, msg->header.stamp.sec);
updateInfo(pose, msg->header.stamp);
}
/*
* purpose : entry to update slam information.
*/
void updateInfo(Sophus::SE3f pose, std_msgs::msg::Header::_stamp_type stamp)
{
std::vector<ORB_SLAM3::MapPoint *>map_points;
/*
* this publishes map and pose.
*/
map_points = slam_->GetTrackedMapPoints();
publishPose(pose, stamp);
publishLocalMapPoints(map_points, stamp);
if (map_mode_ != 0) {
publishGlobalMapPoints(map_points, stamp);
}
}
/*
* purpose : entry to publish pose.
*/
void publishPose(
Sophus::SE3f pose,
std_msgs::msg::Header::_stamp_type stamp
)
{
Eigen::Matrix4f m = pose.matrix().cast<float>();
Eigen::Matrix3f r = m.block<3, 3>(0, 0);
Eigen::Vector3f t = m.block<3, 1>(0, 3);
Eigen::Quaterniond q(r.cast<double>());
auto msg = geometry_msgs::msg::PoseStamped();
geometry_msgs::msg::TransformStamped tf_msg;
msg.header.stamp = stamp;
msg.header.frame_id = camera_frame_id_param_;
/*
* this is to convets orb_slam xyz->rvitx zxy coordinates.
*/
msg.pose.position.x = -t(2);
msg.pose.position.y = t(0);
msg.pose.position.z = t(1);
msg.pose.orientation.x = -q.z();
msg.pose.orientation.y = q.x();
msg.pose.orientation.z = q.y();
msg.pose.orientation.w = q.w();
/*
* this publishes pose.
*/
pose_publisher_->publish(msg);
/*
* this makes tf message.
*/
tf_msg.header.stamp = stamp;
tf_msg.header.frame_id = camera_frame_id_param_;
tf_msg.child_frame_id = base_frame_id_param_;
tf_msg.transform.translation.x = -t(2);
tf_msg.transform.translation.y = t(0);
tf_msg.transform.translation.z = t(1);
tf_msg.transform.rotation.x = -q.z();
tf_msg.transform.rotation.y = q.x();
tf_msg.transform.rotation.z = q.y();
tf_msg.transform.rotation.w = q.w();
tf_broadcaster_->sendTransform(tf_msg);
}
/*
* purpose : entry to publish local map Points.
*/
void publishLocalMapPoints(
std::vector<ORB_SLAM3::MapPoint *>map_points,
std_msgs::msg::Header::_stamp_type stamp
)
{
auto msg = MapPointToPointCloud2(map_points, stamp);
lmp_publisher_->publish(msg);
}
/*
* purpose : entry to publish global map Points.
*/
void publishGlobalMapPoints(
std::vector<ORB_SLAM3::MapPoint *>map_points,
std_msgs::msg::Header::_stamp_type stamp
)
{
ORB_SLAM3::MapPoint *mp;
int n;
/* if the SLAM has been lost his map, this clears our globalmap. */
if (slam_->GetTrackingState() != 2) {
map_.clear();
}
for (n = 0; n < map_points.size(); n++) {
mp = map_points.at(n);
if (mp != (ORB_SLAM3::MapPoint *)0 &&
mp->nObs >= CNF_MIN_OBSERVATION) {
map_.EraseMapPoint(mp);
map_.AddMapPoint(mp);
}
}
auto msg = MapPointToPointCloud2(map_.GetAllMapPoints(), stamp);
mp_publisher_->publish(msg);
}
/*
* purpose : entry to convert MapPoint to PointCloud2.
* returns : converted PointClould2.
*/
sensor_msgs::msg::PointCloud2
MapPointToPointCloud2(
std::vector<ORB_SLAM3::MapPoint *>map_points,
std_msgs::msg::Header::_stamp_type stamp
)
{
sensor_msgs::msg::PointCloud2 cloud;
unsigned char *cloud_data_ptr;
float data_array[CNF_NUM_CHAN];
int n;
cloud.header.stamp = stamp;
cloud.header.frame_id = map_frame_id_param_;
cloud.height = 1;
cloud.width = map_points.size();
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.point_step = CNF_NUM_CHAN * sizeof(float);
cloud.row_step = cloud.point_step * cloud.width;
cloud.fields.resize(CNF_NUM_CHAN);
std::string channel_id[] = {"x", "y", "z"};
for (n = 0; n < CNF_NUM_CHAN; n++) {
cloud.fields[n].name = channel_id[n];
cloud.fields[n].offset = n * sizeof(float);
cloud.fields[n].count = 1;
cloud.fields[n].datatype = sensor_msgs::msg::PointField::FLOAT32;
}
/*
* this resize cloud data array.
*/
cloud.data.resize(cloud.row_step * cloud.height);
cloud_data_ptr = &cloud.data[0];
for (n = 0; n < cloud.width; n++) {
ORB_SLAM3::MapPoint *mp;
mp = map_points.at(n);
if (mp != (ORB_SLAM3::MapPoint *)0 &&
mp->nObs >= CNF_MIN_OBSERVATION) {
/*
* this converts xyz to zxy.
*/
Eigen::Vector3f w = mp->GetWorldPos();
data_array[0] = w(2);
data_array[1] = -w(0);
data_array[2] = -w(1);
memcpy(cloud_data_ptr + (n * cloud.point_step),
data_array, sizeof(data_array));
}
}
return(cloud);
}
};
int main(int argc, char **argv)
{
/* this creates SLAM object. */
ORB_SLAM3::System slam(CNF_VOCAVULARY_PATH, CNF_SETTING_PATH,
ORB_SLAM3::System::MONOCULAR, true);
rclcpp::init(argc, argv);
auto node = std::make_shared<PicarMapMonoNode>(&slam);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
〇 CMakeLists.txt
前回からの修正は、tf2等の必要なパッケージの追加のみです。
cmake_minimum_required(VERSION 3.8)
project(picar_cpp_pkg)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic
-Wno-deprecated -Wno-reorder -Wno-unused-parameter
-Wno-deprecated-declarations -Wno-sign-compare -Wno-pedantic
)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(Pangolin REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(picar_interfaces REQUIRED)
# adds external library header.
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/include
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/include/CameraModels
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/Thirdparty/Sophus
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
)
# adds executable nodes.
add_executable(picar_map_mono src/picar_map_mono.cpp)
ament_target_dependencies(picar_map_mono
rclcpp
sensor_msgs
geometry_msgs
cv_bridge
Pangolin
Eigen3
tf2
tf2_ros
tf2_geometry_msgs
picar_interfaces
)
target_link_libraries(picar_map_mono
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/lib/libORB_SLAM3.so
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/Thirdparty/DBoW2/lib/libDBoW2.so
${CMAKE_CURRENT_SOURCE_DIR}/include/ORB_SLAM3/Thirdparty/g2o/lib/libg2o.so
-lboost_serialization
-lcrypto
)
install(TARGETS
picar_map_mono
DESTINATION lib/${PROJECT_NAME}
)
ament_package()
〇 package.xml
こちらもCMakeLists.txt同様に必要パッケージの追加を行いました。
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>picar_cpp_pkg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ed@todo.todo">ed</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>Pangolin</depend>
<depend>Eigen3</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>picar_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
■ 次回
今回は、ORB_SLAM3を使えるように設計変更を行いました。
次回は、少し推定精度が気になるので、さらに改善を加えようと思います。
また、よろしければ見ていただけるとありがたいです。
