특정 좌표로 이동하기

Model state를 읽어올 준비가 되었으니 이제 로봇을 내 마음대로 움직여보고자 한다.
코드상에 함수를 만들어 함수의 입력값으로 좌표를 입력하면 그 좌표로 로봇이 움직이는 코드를 만들어 보았다. move_func라는 함수를 만들어 입력값으로 로봇의 현재 좌표, 바라보는 각도, 목적지의 좌표를 받는다.

geometry_msgs::Twist move_func(double x, double y, quarternion quar, double dest_x, double dest_y);

return 값으로 로봇의 상태를 반환해야하기 때문에 geometry_msgs::Twist 라는 자료형을 이용하여 함수를 생성했다.

**주의할 점으로 로봇의 각도를 받을때 model_states의 쿼터니언(quarternion)값으로 받게 된다. 코드에서는 로봇의 yaw를 조절하여 로봇을 구동할 것이기 때문에 쿼터니언을 오일러각으로 바꿔주는 과정이 필요하다.

쿼터니언을 오일러각도로 바꿔주기 위해서는 쿼터니언의 w,x,y,z 값이 모두 필요하다. 이 인수들을 그대로 함수에 넣으면 보기에 상당히 좋지 않기 때문에 quarternion 이라는 구조체를 만들어 주었다.

struct quarternion
{
  double w;
  double x;
  double y;
  double z;
};

쿼터니언-오일러 변환공식은 다음과 같다.

여기서 나는 yaw 각도만 필요하기 때문에 Ψ 의 공식을 이용하였다. (q0,q1,q2,q3는 각각 w,x,y,z)

로봇의 회전각도는 로봇의 각도와 목적지까지의 각도의 차가 클때는 빠르게 회전하고 작을때는 느리게 회전하도록 하기 위해 두 각도의 차이에 비례하도록 로봇의 각속도를 주었다.

move_func 함수는 다음과 같다.

geometry_msgs::Twist move_func(double x, double y, quarternion quar, double dest_x, double dest_y){
  quarternion q = quar;
  double angle;
  double vel;
  double t;
  double w;
  t = 1;
  angle = atan2(2*(q.w*q.z+q.x*q.y),1-(2*(pow(q.y,2)+pow(q.z,2))));
  double dest_angle = atan2(dest_y-y,dest_x-x);

  double dist = sqrt(pow((dest_x-x),2)+pow((dest_y-y),2));
  vel = 0.3;
  w = abs(dest_angle-angle)/t;
  ROS_INFO_STREAM("dest_angle:" << dest_angle);
  ROS_INFO_STREAM("robot_angle:" << angle);
  if (dist < 0.1) {
    robot_msg.linear.x = 0;
    robot_msg.angular.z = 0;
  }
  else{

    robot_msg.linear.x = vel;

    if(angle>dest_angle) {
      robot_msg.angular.z = w;
    }
    else {
      robot_msg.angular.z = -w;
    }
    }

  return robot_msg;
}

이렇게 반환한 robot_msg를 publish 하여 로봇을 구동한다.

여기서 또하나의 문제점이 발생하는데 main 함수에서 publisher, subscriber가 선언되어있기 때문에 callback 함수에서 publish를 할수가 없다. 인터넷을 검색해본 결과 클래스를 이용하여 해결할 수 있다고 한다.
class 내부에 public으로 함수를 만들고 private으로 NodeHandle과 publisher, subscriber를 선언한다. 이렇게 함수를 정의하면 class 밖에서도 함수에 접근할 수 있지만 private으로 선언된 것들은 class 내부에서만 이용할 수 있다.
따라서 class로 선언한 코드는 다음과 같다.

class SubscribeAndPublish
{
public:
  SubscribeAndPublish()
  {
    pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    sub_ = n_.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states",10,&SubscribeAndPublish::callback, this);
  }
  void callback(const gazebo_msgs::ModelStates::ConstPtr& msg){


    ROS_INFO_STREAM("robot name:" << msg->name[1]);
    ROS_INFO_STREAM("Position:" << msg->pose[1].position.x << "," << msg->pose[1].position.y << "," << msg->pose[1].position.z);

    quarternion robot_quar;
    robot_quar.w = msg->pose[1].orientation.w;
    robot_quar.x = msg->pose[1].orientation.x;
    robot_quar.y = msg->pose[1].orientation.y;
    robot_quar.z = msg->pose[1].orientation.z;

    robot_msg = move_func(msg->pose[1].position.x, msg->pose[1].position.y, robot_quar, 1, 1);
    pub_.publish(robot_msg);
  }
private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::Subscriber sub_;
};

이렇게 코드를 작성하여 로봇이 (1,1) 지점으로 이동하도록 코드를 실행해 보았다.