制御ソフトウェア

TurtleSimのグラフ図

turtlesimノードと操作UIであるteleop_keyノードはturtle1/cmd_velトピックで接続されており、そのトピックの上を位置・姿勢指令のメッセージが流れます。

位置・姿勢指令のメッセージ構造

teleop_keyノードからturtlesimノードに流れる位置・指令メッセージは以下のような構造を持っています。

geometry_msgs/Twist  ← メッセージ名
  geometry_msgs/Vector3 linear ← ３次元空間の位置
    float64 x  ← X位置
    float64 y  ← Y位置
    float64 z  ← Z位置（今回は２次元平面の為、未使用）
  geometry_msgs/Vector3 angular ← ３次元空間の姿勢
    float64 x  ← X軸回りの回転角（今回は２次元平面の為、未使用）
    float64 y  ← Y軸回りの回転角（今回は２次元平面の為、未使用）
    float64 z  ← Z軸回りの回転角

サンプル制御ソフトウェア

以下のコードはteleop_keyノードから/turtle1/cmd_velトピック上に流れているgeometry_msgsメッセージを受信してモータを動かすデモソフトです。
本来、メッセージに記載されている位置又は速度情報を使って位置・速度制御をしなければなりませんが、以下のコードはturtlesimノードと同じようにうteleop_keyノードからのメッセージで動いていることをデモするためだけに作ったものなので、位置・速度制御は行わずに、一度メッセージを受けたら一定時間モータを回すという処理にして手を抜いています。

// TurtleRealデモ用ソフト

#include <stdio.h>
#include <fcntl.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#define FREQUENCY 20
#define PERIOD 20

#define STOP 0		// 停止
#define ENABLE 1	// ドライバーイネーブル
#define R_TURN 2	// 右回転
#define L_TURN 3	// 左回転
#define FORWARD 4	// 前進
#define BACKWARD 5	// 後進

int t_ctr;
bool set;
double x_pos;;
double z_angle;
int vfd7, vfd8, vfd18, vfd24, vfd25;

// メッセージ受信時コールバック関数
void messageCallBack(const geometry_msgs::Twist& twist) {
	if((x_pos == twist.linear.x) && (z_angle == twist.angular.z)) {
		t_ctr += PERIOD;
	} else {
		x_pos = twist.linear.x;
		z_angle = twist.angular.z;
		t_ctr = PERIOD;
		set = true;
	}
	ROS_INFO_STREAM(x_pos << ", " << z_angle);
}

// GPIO初期化
int initGpio(int port, const char* mode) {
	int efd, dfd, vfd;
	char pstr[10];
	char dstr[128];

	efd = open("/sys/class/gpio/export", O_WRONLY);
	sprintf(pstr, "%d", port);
	write(efd, pstr, strlen(pstr));
	close(efd);

	sprintf(dstr, "/sys/class/gpio/gpio%d/direction", port);
	//ROS_INFO_STREAM(dstr);
	do {
		dfd = open(dstr, O_WRONLY);
	}while(dfd < 0);
	//if(dfd < 0) ROS_INFO_STREAM((std::string)dstr + " open fail");
	write(dfd, mode, strlen(mode));
	close(dfd);
	sprintf(dstr, "/sys/class/gpio/gpio%d/value", port);
	vfd = open(dstr, O_WRONLY);
	if(vfd < 0) ROS_INFO_STREAM((std::string)dstr + " open fail");
	return vfd;
}

// モーター出力
void driveMotor(int mode) {
	switch(mode) {
		case ENABLE:
			ROS_INFO_STREAM("ENABLE !");
			write(vfd18, "1", 1);
			break;
		case R_TURN:
			ROS_INFO_STREAM("R-Trun !");
			write(vfd7, "1", 1);
			write(vfd8, "0", 1);
			write(vfd24, "1", 1);
			write(vfd25, "0", 1);
			break;
		case L_TURN:
			ROS_INFO_STREAM("L-Trun !");
			write(vfd7, "0", 1);
			write(vfd8, "1", 1);
			write(vfd24, "0", 1);
			write(vfd25, "1", 1);
			break;
		case FORWARD:
			ROS_INFO_STREAM("FORWARD !");
			write(vfd7, "1", 1);
			write(vfd8, "0", 1);
			write(vfd24, "0", 1);
			write(vfd25, "1", 1);
			break;
		case BACKWARD:
			ROS_INFO_STREAM("BACKWARD !");
			write(vfd7, "0", 1);
			write(vfd8, "1", 1);
			write(vfd24, "1", 1);
			write(vfd25, "0", 1);
			break;
		case STOP:
		default:
			ROS_INFO_STREAM("STOP !");
			write(vfd7, "0", 1);
			write(vfd8, "0", 1);
			write(vfd24, "0", 1);
			write(vfd25, "0", 1);
			break;
	}
}

// 制御メイン
int main(int argc, char **argv) {
	ROS_INFO_STREAM("TurtleReal control start!");
	ros::init(argc, argv, "turtlereal_node");
	ros::NodeHandle nh;

	// ポートオープン・初期設定
	vfd7 = initGpio(7, "out");
	vfd8 = initGpio(8, "out");
	vfd18 = initGpio(18, "out");
	vfd24 = initGpio(24, "out");
	vfd25 = initGpio(25, "out");
	driveMotor(ENABLE);
	driveMotor(STOP);
	ROS_INFO_STREAM("GPIO initialized");

	set = false;
	x_pos = 0;
	z_angle = 0;
	ros::Subscriber sub = nh.subscribe("/turtle1/cmd_vel", 1000, &messageCallBack);  // サブスクライバ生成
	ros::Rate rate(FREQUENCY);		// 周期設定
  
 	// Ctrl+Cが押されるまで繰り返し
	while(ros::ok()) {
		if(set) {
			if((x_pos == 0) && (z_angle == 0)) driveMotor(STOP);
			else {
				if(z_angle != 0) {
					if(z_angle > 0) driveMotor(L_TURN);
					else driveMotor(R_TURN);
				}
				if(x_pos != 0) {
					if(x_pos > 0) driveMotor(FORWARD);
					else driveMotor(BACKWARD);
				}
			}
			set = false;
		}
		if(t_ctr == 1) {
			t_ctr = 0;
			x_pos = 0;
			z_angle = 0;
			set = true;
		} else t_ctr--;
		ros::spinOnce();
		rate.sleep();
	}
}

<< 前のページ次のページ >>