[ROS] Gazebo상에서 rosservice를 이용해서 joint position값 받기

1. rqt를 이용하여 rosservice 값 받기

 

앞서 포스트한 글을 통해 Gazebo에서 매니퓰레이터가 달린 이동로봇을 불러온 뒤

rqt

를 실행한 뒤 화면 좌상단에 Plugins/services/Service Caller를 클릭하면

 

Gazebo에서 주고 받을 수 있는 Request와 Response를 확인할 수 있다.

 

우리는 조인트 값을 받는게 목표이므로 Service에서 /gazebo/get_joint_properties를 선택하고 Call을 누르면

 

이 서비스에서 주고 받을 수 있는 Request와 Response를 알 수 있다.

 

Request에 있는 Joint name에 Expression에 본인이 값을 받고 싶은 조인트의 이름을 입력해준다.

 

필자는 조인트 이름이 Body이므로 '' --> 'Body'로 수정했다.

 

이렇게 한 뒤 Call을 누르면 가제보상의 Body이름을 가진 조인트의 position 값을 받을 수 있다.

 

---

2. 터미널 상에서 Joint_position 값 받기

 

터미널 창에 다음과 같은 명령어를 입력한다.

rosservice call /gazebo/get_joint_properties 'body'

 

이를 통해 좀전에 rqt에서 봤던 Response 내용들을 확인할 수 있다.

 

하지만 우리는 이런 방식이 아닌 코드 안에서 service 값을 직접 불러봐야 한다.

---

3. 코드를 작성해서 joint_position 값 받기

기존의 teleop_twist_keyboard.cpp 파일을 토대로 하고, 다음 사이트를 참고하면서 코드를 작성해보았다.

 

https://www.cse.sc.edu/~jokane/agitr/agitr-small-service.pdf

3-1. teleop_twist_keyboard_cpp 파일 수정

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

#include <stdio.h>
#include <unistd.h>
#include <termios.h>

#include <map>

#include <gazebo_msgs/GetJointProperties.h>

#include <unistd.h>

int main(int argc, char** argv)
{
  // Init ROS node
  ros::init(argc, argv, "teleop_twist_keyboard");
  ros::NodeHandle nh;

  // Init cmd_vel publisher
  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

  // Create Twist message
  geometry_msgs::Twist twist;

  //새로 추가된 부분
  ros::ServiceClient gazebo_Client = nh.serviceClient<gazebo_msgs::GetJointProperties>
  ("gazebo/get_joint_properties");

  gazebo_msgs::GetJointProperties::Request req;
  gazebo_msgs::GetJointProperties::Response resp;

  req.joint_name = "link_joint";


  while(true){
    
    bool success = gazebo_Client.call(req, resp);

    if(success) {
      ROS_INFO("\rJoint position is %f", resp.position[0]);
    } else {
      ROS_ERROR("\rfailed to spawn");
    }
   
    // Publish it and resolve any remaining callbacks
    pub.publish(twist);
    ros::spinOnce();
  }
  return 0;
}

 

코드 윗 부분에서 

#include <gazebo_msgs/GetJointProperties.h>

가 추가 되었고

 

 

main문 안에서는 

  ros::ServiceClient gazebo_Client = nh.serviceClient<gazebo_msgs::GetJointProperties>
  ("gazebo/get_joint_properties");

  gazebo_msgs::GetJointProperties::Request req;
  gazebo_msgs::GetJointProperties::Response resp;

  req.joint_name = "link_joint";

다음과 같은 코드를 통해 조인트값을 rosservice로 받을 수 있다.

 

 

while문에서는

while(true){
    
    bool success = gazebo_Client.call(req, resp);

    if(success) {
      ROS_INFO("\rJoint position is %f", resp.position[0]);
    } else {
      ROS_ERROR("\rfailed to spawn");
    }
   
    // Publish it and resolve any remaining callbacks
    pub.publish(twist);
    ros::spinOnce();
  }

를 통해 받은 받은 터미널상에서 print할 수 있다.

 

---

3-2. CMakeLists.txt 파일 수정

CMakeLists.txt파일은 

cmake_minimum_required(VERSION 2.8.3)
project(teleop_twist_keyboard_cpp)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
)

catkin_package(
  INCLUDE_DIRS src
  CATKIN_DEPENDS roscpp geometry_msgs
)

include_directories(${catkin_INCLUDE_DIRS})

add_executable(teleop_twist_keyboard src/teleop_twist_keyboard.cpp)
target_link_libraries(teleop_twist_keyboard ${catkin_LIBRARIES})

set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")

다음과 같이 수정을 해주었다.

아마 기존의 파일에서

add_executable(teleop_twist_keyboard src/teleop_twist_keyboard.cpp)

이 부분만 추가했던걸로 기억함..

---

3-3. Package.xml 파일 수정

그리고 packge.xml 파일도

<?xml version="1.0"?>
<package format="2">
  <name>teleop_twist_keyboard_cpp</name>
  <version>0.0.0</version>
  <description>Generic keyboard teleop for twist robots (in C++)! Based off of the teleop_twist_keyboard Python ROS node.</description>

  <author email="methyldragon@gmail.com">methylDragon</author>
  <maintainer email="methyldragon@gmail.com">methylDragon</maintainer>
  <url type="website">http://github.com/methylDragon</url>
  <url>http://www.ros.org/wiki/teleop_twist_keyboard_cpp</url>

  <license>MIT</license>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>
  <build_depend>geometry_msgs</build_depend>

  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>

  <exec_depend>roscpp</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  
</package>

다음처럼 작성해준다. 수정한 부분이 있었는지 기존의 것을 그대로 썼는지 기억이 잘 안 나서 일단 올린다.

 

---

3-3. 빌드 후 실행

cd ..
cd catkin_ws
catkin build
roslaunch mobile_robot mobile_robot_gazebo.launch
rosrun teleop_twist_keyboard_cpp teleop_twist_keyboard

사진처럼 0.01초 간격으로 조인트 값이 나오면 잘 작동하는 것이다.