AR 마커로 TurtleSim 제어하기

전 포스팅에 이어서 작성해보려고 한다 https://noel-embedded.tistory.com/1175

 

카메라에 비친 AR마커 위치에 따라서 Turtlesim이 움직이도록 하는 코드를 작성해보려고 한다

따라서, 카메라 - AR마커 alvar - 새 작성 노드 - turtlesim으로 노드 구성을 하려고 한다

 

터틀심 속도를 publish를 하고,

 

속도 제어를 하면서, 동시에 pose를 실시간으로 체크한다

 

회전과 병진속도는 각각 z, x 축에 의해서만 움직인다. 즉 전방은 x축, yaw는 z축을 의미한다

 

pose 값은 다음과 같이 발행이 된다. 왼쪽 아래는 (0, 0), 오른쪽 위는 (11, 11)이다

 

다음은 AR 마커 토픽에 대한 내용이다. 이 토픽을 Subscribe를 할 것이다

$ catkin_create_pkg ar_marker_tts roscpp std_msgs geometry_msgs message_generation ar_track_alvar_msgs sensor_msgs

 

아래 내용을 CMakeList에 추가해준다

add_executable(control src/control.cpp)

add_dependencies(control ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(control
   ${catkin_LIBRARIES}
)

 

코드는 다음과 같다. 비례상수가 맞지 않는다면 조절해서 대입할 수 있다

	#include "ros/ros.h"
	#include "turtlesim/Pose.h"
	#include "std_msgs/Float64.h"
	#include "geometry_msgs/Twist.h"
	#include "ar_track_alvar_msgs/AlvarMarker.h"
	#include "ar_track_alvar_msgs/AlvarMarkers.h"
	#include <cmath>
	#include <cstdlib>
	#define MARKER_ID 9
	geometry_msgs::Twist pub_msg;
	turtlesim::Pose goal_pose, cur_pose;
	void poseCallback(const turtlesim::Pose::ConstPtr& msg)
	{
	#if 0
	ROS_INFO("x : [%f]", msg->x);
	ROS_INFO("y : [%f]", msg->y);
	ROS_INFO("theta : [%f]", msg->theta);
	ROS_INFO("linear_velocity : [%f]", msg->linear_velocity);
	ROS_INFO("angular_velocity : [%f]", msg->angular_velocity);
	#endif
	cur_pose.x = msg->x;
	cur_pose.y = msg->y;
	cur_pose.theta = msg->theta;
	}
	void arposeCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
	{
	for(int i=0; i<msg->markers.size(); i++) {
	#if 0
	ROS_INFO("posex : [%f]", msg->markers[i].pose.pose.position.x);
	ROS_INFO("posey : [%f]", msg->markers[i].pose.pose.position.y);
	ROS_INFO("posez : [%f]", msg->markers[i].pose.pose.position.z);
	ROS_INFO("quatx : [%f]", msg->markers[i].pose.pose.orientation.x);
	ROS_INFO("quaty : [%f]", msg->markers[i].pose.pose.orientation.y);
	ROS_INFO("quatz : [%f]", msg->markers[i].pose.pose.orientation.z);
	ROS_INFO("quatw : [%f]", msg->markers[i].pose.pose.orientation.w);
	#endif
	if( i==0 && msg->markers[i].id == MARKER_ID) {
	float x = msg->markers[i].pose.pose.position.x;
	float y = msg->markers[i].pose.pose.position.y;
	if(x < -0.2) {
	if(y < -0.15) {
	goal_pose.x = 2.0;
	goal_pose.y = 8.0;
	}
	else if(y > 0.15) {
	goal_pose.x = 2.0;
	goal_pose.y = 2.0;
	}
	else {
	goal_pose.x = 2.0;
	goal_pose.y = 5.5;
	}
	}
	else if(x <= 0.2 && x >= -0.2) {
	if(y < -0.15) {
	goal_pose.x = 5.5;
	goal_pose.y = 8.0;
	}
	else if(y > 0.15) {
	goal_pose.x = 5.5;
	goal_pose.y = 2.0;
	}
	else {
	goal_pose.x = 5.5;
	goal_pose.y = 5.5;
	}
	}
	else {
	if(y < -0.15) {
	goal_pose.x = 8.0;
	goal_pose.y = 8.0;
	}
	else if(y > 0.15) {
	goal_pose.x = 8.0;
	goal_pose.y = 2.0;
	}
	else {
	goal_pose.x = 8.0;
	goal_pose.y = 5.5;
	}
	}
	}
	}
	#if 0
	ROS_INFO("x : [%f]", msg->x);
	ROS_INFO("y : [%f]", msg->y);
	ROS_INFO("theta : [%f]", msg->theta);
	ROS_INFO("linear_velocity : [%f]", msg->linear_velocity);
	ROS_INFO("angular_velocity : [%f]", msg->angular_velocity);
	#endif
	}
	double distanceToGoal(void) {
	return sqrt(pow((goal_pose.x-cur_pose.x), 2) + pow((goal_pose.y-cur_pose.y), 2));
	}
	int main(int argc, char **argv) {
	double x, y, tolerance;
	ros::init(argc, argv, "control");
	ros::NodeHandle nh;
	ros::Rate loop_rate(10);
	ros::Publisher cmdvel_pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 100);
	ros::Subscriber pose_sub = nh.subscribe("/turtle1/pose", 100, poseCallback);
	ros::Subscriber ar_pose_sub = nh.subscribe("/ar_pose_marker", 100, arposeCallback);
	goal_pose.x = cur_pose.x = 5.544445;
	goal_pose.y = cur_pose.y = 5.544445;
	geometry_msgs::Twist vel;
	vel.linear.x = 0;
	vel.linear.y = 0;
	vel.linear.z = 0;
	vel.angular.x = 0;
	vel.angular.y = 0;
	vel.angular.z = 0;
	cmdvel_pub.publish(vel);
	while(ros::ok()) {
	#if 0
	ROS_INFO("cur_pose.x : [%f]", cur_pose.x);
	ROS_INFO("cur_pose.y : [%f]", cur_pose.y);
	ROS_INFO("goal_pose.x : [%f]", goal_pose.x);
	ROS_INFO("goal_pose.y : [%f]", goal_pose.y);
	#endif
	loop_rate.sleep();
	ros::spinOnce();
	double kp1 = 0.5, kp2 = 3.0;
	double e = distanceToGoal();
	ROS_INFO("e : [%lf]", e);
	while(e >= 0.1) {
	#if 1
	ROS_INFO("cur_pose.x : [%f]", cur_pose.x);
	ROS_INFO("cur_pose.y : [%f]", cur_pose.y);
	ROS_INFO("goal_pose.x : [%f]", goal_pose.x);
	ROS_INFO("goal_pose.y : [%f]", goal_pose.y);
	#endif
	vel.linear.x = kp1*e;
	vel.linear.y = 0;
	vel.linear.z = 0;
	vel.angular.x = 0;
	vel.angular.y = 0;
	vel.angular.z = kp2*(atan2(goal_pose.y-cur_pose.y, goal_pose.x-cur_pose.x)-cur_pose.theta);
	cmdvel_pub.publish(vel);
	loop_rate.sleep();
	ros::spinOnce();
	e = distanceToGoal();
	}
	vel.linear.x = 0;
	vel.angular.z = 0;
	cmdvel_pub.publish(vel);
	}
	return 0;
	}

view raw ar_marker_turtlesim.cpp hosted with ❤ by GitHub

 

아래 명령어를 순차적으로 실행한다

$ roscore
$ rosrun turtlesim turtlesim_node
$ roslaunch ar_marker track_marker.launch 
$ rosrun uvc_camera uvc_camera_node
$ rosrun ar_marker_tts control
$ rqt_image_view

 

ㅌ