ROS 2を使ってジャイロオドメトリのノードを作成します。ROS1のときは、アナログ・デバイセズの1軸角速度センサをI2CでArduinoと通信して、Arduino内で積分した角度をUDPで送信したものを受信するノードを作成して、モータドライバから車輪の回転角度を受信したものと合わせてジャイロオドメトリの計算を行っていました。
今回は、RT社のIMUを利用して、すでにRT社が提供しているROS2のノードからPublishされている/Imu/raw_dataのトピックをSubscribeしてジャイロオドメトリを計算するプログラムを作成します。
準備するもの
早速プログラム。
$ cd ~/ros2_ws/src/
$ mkdir mobile_robot ＃取りあえずmobile_robotというディレクトリを作りました。作らなくてもOK。
$ cd mobile_robot
$ ros2 pkg create --build-type ament_cmake --node-name gyro_odom_node odometory #odometoryというパッケージ内にgyro_odom_node作成
$ cd odometory/src
$ gedit gyro_odom_node.cpp
下記がコード。
// gyro_odom_node.cpp
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "geometry_msgs/msg/twist.hpp"
using namespace std::chrono_literals;
class GyroOdometer : public rclcpp::Node
{
public:
GyroOdometer()
: Node("gyro_odometer")
{
subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
"imu/raw_data", 10, std::bind(&GyroOdometer::imu_callback, this, std::placeholders::_1));
publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("odom", 10);
timer_ = this->create_wall_timer(50ms, std::bind(&GyroOdometer::timer_callback, this));
}
private:
void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
// ジャイロオドメトリの計算
//double dt = (msg->header.stamp - last_time_).seconds();
double dt = (rclcpp::Time(msg->header.stamp) - last_time_).seconds();
double angle_velocity_z = msg->angular_velocity.z;
double linear_velocity_x = msg->linear_acceleration.x * dt;
double linear_velocity_y = msg->linear_acceleration.y * dt;
double linear_velocity_z = msg->linear_acceleration.z * dt;
double angular_velocity_z = angle_velocity_z * dt;
// Twistメッセージを作成
auto twist = geometry_msgs::msg::Twist();
twist.linear.x = linear_velocity_x;
twist.linear.y = linear_velocity_y;
twist.linear.z = linear_velocity_z;
twist.angular.z = angular_velocity_z;
// Twistメッセージをpublish
publisher_->publish(twist);
last_time_ = msg->header.stamp;
}
void timer_callback()
{
RCLCPP_INFO(this->get_logger(), "GyroOdometer running...");
}
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time last_time_{0};
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<GyroOdometer>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}