1つのプログラム中に2つのノードを作るとマルチスレッドのプログラムが作れます。
移動ロボットの制御にROS1ではArduinoMEGAを使っていたのでROS2でも使いたくて、/cmd_velをsubscribeして、その速度指令をUSBシリアルで送信、一定周期で返ってくる車輪のエンコーダ値をUSBシリアルで受信して、回転角から得られる自己位置をpublishすることを考えていました。。。
結論から言うと、今のところシリアル回りで通信が固まってしまいますが、マルチスレッドとしては動いているようなのでメモを残します。
とりあえずノードを下記のように作成。
$ cd ~/ros2_ws/src/
$ ros2 pkg create --build-type ament_cmake --node-name serial_node serial
$ cd serial/src
$ gedit serial_node.cpp
serial_node.cppの中身は下記の通り。
serial_inノードとserial_outノードの2つをクラスで作成してexec.add_nodeでスレッドを生成することができる。
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <functional>
#include <string>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <chrono>
#include <memory>
using std::placeholders::_1;
using namespace std::chrono_literals;
#define BAUDRATE B9600 //ボーレート
int fd1;
int sem = 0; //セマフォ
double vel[3]={0,0,0};
class Serial_in : public rclcpp::Node //Serian_inクラス
{
public:
Serial_in()
: Node("Serial_in")
{
subscription_ = this->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel", 10, std::bind(&Serial_in::topic_callback, this, _1));//cmd_vel subscribe
}
private:
void topic_callback(const geometry_msgs::msg::Twist::SharedPtr msg) const
{
// while(sem==1) usleep(1000);//5ms sleep
// sem=1; //セマフォを1にする
//int count = 0;
//char buf[32];//ArduinoUNOの場合64バイト以下にすること。
//for(int i = 0; i<32; i++) buf[i]='\0';
RCLCPP_INFO(this->get_logger(), "I heard: '%lf' '%lf'", msg->linear.x, msg->angular.z);
vel[0] = msg->linear.x;
vel[2] = msg->angular.z;
/*
sprintf(buf, "%7.5f,%7.5f\n",msg->linear.x, msg->angular.z);
for(int i = 0;i<(int)sizeof(buf);i++){
if(buf[i]=='\n'){ count=i; break;}
}
std::cout << "count = " << count << std::endl;
//printf("cmd_vel recv:%s\n",buf);
int rec=write(fd1,buf,count+1);
if(rec>=0)printf("Serial send:%s\n",buf);
else{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Serial Fail: could not write");
}
// sem=0;
*/
//usleep(5000);//5ms sleep
}
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscription_;
};
class Serial_out : public rclcpp::Node //Serial_outクラス
{
public:
Serial_out() : Node("Serial_out"), count_(0) {
count_ = 0;
publisher_ = this->create_publisher<std_msgs::msg::String>("Serial_out", 10);//Serial_outトピックをpublish
timer_ = this->create_wall_timer(std::chrono::milliseconds(10),//10ms毎のループ std::bind(&Serial_out::topic_callback, this));
}
private:
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
size_t count_;
void topic_callback() {
char buf[256]={0};
std::string data;
int len=0;//受信文字の長さチェックのため
int flag = 0;//受信有無のフラグ
int count = 0;
//serial send
sprintf(buf, "%7.5f,%7.5f\n",vel[0], vel[2]);
for(int i = 0;i<(int)sizeof(buf);i++){
if(buf[i]=='\n'){ count=i; break;}
}
std::cout << "count = " << count << std::endl;
//printf("cmd_vel recv:%s\n",buf);
int rec=write(fd1,buf,count+1);
if(rec>=0)printf("Serial send:%s\n",buf);
else{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Serial Fail: could not write");
}
usleep(4000);
// while(sem==1) usleep(1000);//1ms sleep
// sem=1;//セマフォを1にする
//serial receive
while(true){//改行コードを見つけるまで無限ループ
int recv_data=read(fd1, buf, sizeof(buf));//届いたデータ読み込み
if(recv_data>0){
flag=1;//1回目の受信用のフラグ
len = len + recv_data;
//buf[recv_data]='\0';
data += std::string(buf, recv_data);
if(data.back()=='\n'){//改行コードがあったら文字列表示
//printf("recv:%03d %s\n",len, data);
std::cout << "recv: "<< len << " " << data << std::endl;
if(len>1){//改行コードだけじゃないときpublish
auto serial_msg = std::make_unique<std_msgs::msg::String>();
serial_msg->data = data;//buf;
publisher_->publish(std::move(serial_msg));
//serial_pub->publish(std::move(serial_msg));
}
break;
}
}else{//1回目の受信長がゼロならブレイク
if(flag==0) break;
}
}
// sem=0;//セマフォを0にする
// auto message = std_msgs::msg::String();
// message.data = "Hello, world2! " + std::to_string(count_++);
// RCLCPP_INFO(this->get_logger(), "Publishing2: '%s'", message.data.c_str());
// publisher_->publish(message);
}
};
// シリアルポートの初期化
void serial_init(int fd) {
struct termios tio;
memset(&tio, 0, sizeof(tio));
tio.c_cflag = CS8 | CLOCAL | CREAD;
tio.c_cc[VTIME] = 0;
tio.c_lflag = ICANON;
tio.c_iflag = IGNPAR | ICRNL;
cfsetispeed(&tio, BAUDRATE);
cfsetospeed(&tio, BAUDRATE);
tcsetattr(fd, TCSANOW, &tio);
}
int main(int argc, char **argv){
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor exec;
char device_name[]="/dev/ttyACM0";//ArduinoUNOのデバイス
//fd1=open_serial(device_name);
fd1 = open(device_name, O_RDWR | O_NOCTTY);
if(fd1<0){
// RCLCPP_ERROR(node->get_logger(), "Serial Fail: could not open %s", device_name);
printf("Serial Fail\n");
rclcpp::shutdown();
}
serial_init(fd1);
auto serial_in = std::make_shared<Serial_in>();
auto serial_out = std::make_shared<Serial_out>();
exec.add_node(serial_in); //serial_inノード
exec.add_node(serial_out); //serial_outノード
exec.spin();
rclcpp::shutdown();
return 0;
}
このプログラムだと何回かwriteで書き込んでいると固まります。
シリアル通信をしなければマルチスレッドの参考になるかもしれない。。。
C++のシリアルがどうしてもうまくいかないのでPythonで試してみたいと思います。。。
Pythonでのノードを下記のように作成します。
$ cd ~/ros2_ws/src/
$ ros2 pkg create --build-type ament_python --node-name pyserial_node pyserial
$ cd pyserial/pyserial
$ gedit pyserial.py
pyserial.pyの内容は下記の通り。
まずは，pubとsubのサンプルを同時にマルチスレッドで動かせることの確認のプログラム．
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from rclpy.executors import MultiThreadedExecutor
class Publisher(Node):
def __init__(self):
super().__init__('mypublisher')
self.publisher_ = self.create_publisher(String, 'topic', 10) # topicというトピック名でデータを送信。バッファサイズは10。
timer_period = 0.5 # 0.5秒ごとに送信する設定
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0
def timer_callback(self):
msg = String()
msg.data = 'Hello World: %d' % self.i
self.publisher_.publish(msg) # 送信
self.get_logger().info('Publishing: "%s"' % msg.data) # 送信したデータをコンソールに表示
self.i += 1
class Subscriber(Node):
def __init__(self):
super().__init__('mysubscriber')
self.subscription = self.create_subscription(
String,
'topic',
self.listener_callback,
10)
self.subscription # prevent unused variable warning
def listener_callback(self, msg):
self.get_logger().info('I heard: "%s"' % msg.data)
def main(args=None):
rclpy.init(args=args)
exec = MultiThreadedExecutor()
publisher_node = Publisher()
subscriber_node = Subscriber()
exec.add_node(publisher_node)
exec.add_node(subscriber_node)
exec.spin()
exec.shutdown()
publisher_node.destroy_node()
subscriber_node.destroy_node()
rclpy.shutdown()
上記を実行すると同時のpubとsubができるのが確認できるはず．
これを変更してC++でうまくいかなかったシリアル通信のプログラムを作ってみる．
漸くできたプログラムが下記．全然整理できていませんが動いたのでメモ代わりにアップ．
import serial
import time
import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class SerialCommunication(Node):
def __init__(self):
super().__init__('serial_communication')
self.serial_port = serial.Serial('/dev/ttyACM0', 9600, timeout=0.01) #ArduinoUNOは9600が良さそう
self.serial_port.reset_input_buffer() #バッファクリア
self.serial_port.reset_output_buffer() #バッファクリア
self.get_logger().info('Serial port opened')
self.timer_period = 0.01 # seconds
self.timer = self.create_timer(self.timer_period, self.timer_callback)
self.wheel_velocities = [0.0, 0.0]
self.robot_pose = [0.0, 0.0, 0.0]
self.subscription = self.create_subscription(
Twist,
'/cmd_vel',
self.twist_callback,
10
)
def timer_callback(self):
# Send wheel velocities to the robot
# wheel_velocities_str = '{:.2f},{:.2f}\n'.format(
# self.wheel_velocities[0],
# self.wheel_velocities[1]
# )
wheel_velocities = [self.wheel_velocities[0], self.wheel_velocities[1]]
wheel_velocities_str = ','.join(str(v) for v in wheel_velocities)
self.serial_port.write(wheel_velocities_str.encode())
self.get_logger().info("Sent wheel velocities: %s" % wheel_velocities_str)
# Receive robot pose from the robot
data = self.serial_port.readline().decode(errors='ignore').strip('\0')
if data:
self.get_logger().info("Received data: %s" % data)
try:
wheel = [float(x) for x in data.split(',')]
if len(wheel) != 2:
return None
left_wheel_degrees = wheel[0]
right_wheel_degrees = wheel[1]
self.update_robot_pose(left_wheel_degrees, right_wheel_degrees)
except ValueError:
self.get_logger().warning('Failed to convert data to floats: %s' % data)
return None
def twist_callback(self, msg):
# Convert twist message to wheel velocities
linear_velocity = msg.linear.x
angular_velocity = msg.angular.z
left_wheel_velocity = linear_velocity - angular_velocity
right_wheel_velocity = linear_velocity + angular_velocity
self.wheel_velocities = [left_wheel_velocity, right_wheel_velocity]
def update_robot_pose(self, left_wheel_degrees, right_wheel_degrees):
# Update robot pose using wheel degrees
wheel_distance = 0.2 # Distance between wheels in meters
wheel_radius = 0.05 # Wheel radius in meters
wheel_circumference = 2.0 * 3.14159 * wheel_radius
left_distance = wheel_circumference * (left_wheel_degrees / 360.0)
right_distance = wheel_circumference * (right_wheel_degrees / 360.0)
delta_distance = (left_distance + right_distance) / 2.0
delta_theta = (right_distance - left_distance) / wheel_distance
delta_theta_rad = math.radians(delta_theta)
self.robot_pose[0] += delta_distance * \
(self.robot_pose[2] + delta_theta_rad / 2.0)#.cos()
self.robot_pose[1] += delta_distance * \
(self.robot_pose[2] + delta_theta_rad / 2.0)#.sin()
self.robot_pose[2] += delta_theta
# Publish robot pose as a tf2 transform and a PoseStamped message
# ...
def main(args=None):
rclpy.init(args=args)
node = SerialCommunication()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
このプログラムと通信していたArduinoUNOのプログラムは下記．
100Hzくらいでやり取りできているはず．．．
９６００ｂｐｓじゃないと安定しない
volatile uint8_t flag_firsttime = 1;
void setup() {
// シリアル通信の初期化
Serial.begin(9600);//9600bps
Serial.setTimeout(10); //タイムアウト短縮1sから0.01sへ
}
#define ARRAY_NUM 2 //Twist型の要素＝２とする
static String array[ARRAY_NUM];
void loop() {
String buf;
int len=0;//カンマ区切りの数字の数カウントのため
if ( Serial.available() ) {
String str = Serial.readStringUntil('\n');
Serial.println(str);
double data[5];
len = stringToDoubleValues( str, data, ',' );
if(sizeof(data)<len) len = sizeof(data);
buf = data[0]*10;
for(int i = 1; i < len; i++){
buf +=",";
buf += data[i]*10.0; ;
}
Serial.println(buf);
}else{
Serial.println("100,100");
}
delay(10);//10ms
}
// String型のカンマ区切り文字列をdouble型配列に分解する関数
#define MAX_SIZE 256
int stringToDoubleValues(String str, double value[], char delim) {
int k = 0;
int j = 0;
char text[MAX_SIZE];
for (int i = 0; i <= str.length(); i++) {
char c = str.charAt(i);
if ( c == delim || i == str.length() ) {
text[k] = '\0';
if(k<sizeof(text)) value[j] = atof(text);
j++;
k = 0;
} else {
text[k] = c;
k++;
}
}
return j;
}