Gazebo model state 읽어들이기

길찾기 알고리즘을 구현하기 위해 맵 상의 장애물의 위치와 로봇의 현재 위치가 필요하다. 따라서 Gazebo 내에 존재하는 모델의 parameter들을 읽어야 코드를 구현 할 수 있는데 오늘은 이 parameter들을 읽는 방법에 대해 알아보았다. 우선

rostopic list

명령어를 실행하면
이와 같이 토픽들을 볼 수 있다. 여기서 내가 읽어야 하는 토픽은 /gazebo/model_states 이다.

rostopic info /gazebo/model_states

를 입력하면 이와같이 메시지타입을 알 수 있다.
QtCreator로 프로젝트를 로드해서

#include "gazebo_msgs/ModelStates.h"

를 찾을 수 있다.
subscriber를 만들고 callback 함수를 만든다.

void chattercallback(const gazebo_msgs::ModelStates::ConstPtr& msg){
  ROS_INFO_STREAM(msg->name[0]);
}
  ros::Subscriber chatter_sub = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states",1000,chattercallback);

여기서 callback 함수에서 받는 msg->name이 Gazebo에 존재하는 모델들의 이름이다. 이와같이 모든 parameter들은 배열형태로 존재한다. 현재 가제보에는 world(ground)와 로봇밖에 없기 때문에 배열의 인덱스는 0과 1밖에 없다.

rosrun move_test test_pub

을 실행하면
이런식으로 Gazebo의 ModelStates를 읽어올 수 있다.