Skip to content

Indy와 ROS 연결하기

간단한 예제를 통하여 Indy 로봇을 ROS를 이용하여 움직이는 방법을 설명합니다.

요구 환경

  • Ubuntu 16.04 및 ROS Kinetic 이상 이 설치된 PC

  • indy-ros 소스코드 설치

  • 인디 프레임워크 소프트웨어 버전 2.3.1 이상

위 요구 환경의 설치가 완료되지 않은 경우 설치를 먼저 진행하여 주십시오.

실행 방법

1. 터미널 열기

2. ROS 환경설정 확인하기

ROS 명령어를 정상적으로 사용하기 위하여 terminal 의 환경에 ROS 환경변수를 추가 할 필요가 있습니다. 아래의 명령어로 추가할 수 있으며, 설치에서 설명한 bashrc에 해당 명령어들이 추가되어 있을 경우, 이 과정을 생략 할 수 있습니다.

1
2
source /opt/ros/<사용중인 ROS 버전>/setup.bash
source <작업중인 워크스페이스 경로>/devel/setup.bash

3. 아래 명령어를 실행하여 dcp driver와 rviz를 실행합니다. 192.168.x.x는 로봇의 IP이며 robot_name은 로봇의 이름입니다(RP2의 경우 NRMK-IndyRP2). 사용자의 환경에 맞는 적절한 값을 입력해야합니다.

1
roslaunch indy7_dcp_example indy7.launch robot_ip:= 192.168.x.x robot_name:=NRMK-Indy7 


4. 별도의 터미널에서 app.py 파일을 실행합니다.


5. 0번째 관절부터 순서대로 목표 관절 위치를 입력합니다. 단위는 degree입니다.

Warning

실수로 인해 로봇이 충돌할 수 있습니다. 처음에는 0, 0, 0, 0, 0, 0를 입력하여 영 위치로 이동시키는것을 추천합니다.


6. 'g'를 입력하여 로봇이 움직임을 확인합니다.


indy_driver_py 패키지

패키지 구성

1
2
3
4
5
6
7
├── indy_driver_py
    ├── launch/
        └── indy_interface_py.launch
    ├── src/
        ├── indydcp/
        └── dcp_driver.py
    └── ...
  • dcp_driver.py : dcp_drive 의 핵심 프로그램입니다. ROS에서 전달되는 move명령이나 목표 값 등을 IndyDCP 의 명령으로 변환하며, 로봇의 현재상태를 주기적으로 ROS 시스템에 전달하는 역할을 합니다.

  • indy_interface_py.launch : 입력받은 로봇의 이름, IP를 ROS 파라미터로 등록하고 dcp_driver.py를 실행합니다.

dcp_driver.py

아래의 코드는 dcp_driver.py 의 메인 함수 부분입니다.

1
2
3
4
5
def main():
    robot_ip = rospy.get_param("robot_ip_address")
    robot_name = rospy.get_param("robot_name")
    t = IndyROSConnector(robot_ip, robot_name)
    t.run()

ROS 파라미터로 등록된 IP와 로봇이름을 읽어와 IndyROSConnector를 초기화합니다. 그 후 IndyROSConnector 의 run() 메서드를 호출합니다.

아래에서 IndyROSConnector를 설명합니다. IndyROSConnector의 전문은 패키지를 다운받아서 확인할 수 있습니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
def __init__(self, robot_ip, robot_name):
    self.robot_name = robot_name

    # Connect to Robot
    self.indy = indydcp_client.IndyDCPClient(robot_ip, robot_name)
    self.indy.connect()

    # Initialize ROS node
    rospy.init_node('indy_driver_py')
    self.rate = rospy.Rate(50) # hz

    self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
    self.task_state_pub = rospy.Publisher('/indy/task_pos', Float64MultiArray, queue_size=10)

    self.query_joint_state_sub = rospy.Subscriber("/indy/query_joint_states", DisplayRobotState,  self.joint_state_callback, queue_size=10)
    self.plan_result_sub = rospy.Subscriber("/move_group/result", MoveGroupActionResult, self.plan_result_callback, queue_size=1)
    self.execute_plan_sub = rospy.Subscriber("/indy/execute_plan", Bool, self.execute_plan_callback, queue_size=1)
    self.stop_sub = rospy.Subscriber("/indy/stop_robot", Bool, self.stop_robot_callback, queue_size=1)

robot_ip 는 네트워크에 연결된 CB의 ip, name은 로봇의 이름입니다. line 5에서 dcp client 에 입력한 값들을 전달 한 후, line 6에서 로봇에 연결을 시도합니다.

다음은 ROS node의 설정 부분입니다. line 10에서 rospy의 init_node를 사용해 이름이 'indy_driver_py'인 ROS노드를 생성합니다. line 11에서 노드를 주기적으로 실행하기 위해 rate를 50 hz로 설정했습니다. dcp driver의 역할은 Indy의 현재 관절 위치를 ROS에 전달하고, ROS의 명령을 받아 Indy에 전달하는 것입니다.

역할 수행을 위해 현재관절상태(joint_states)를 발신(publish)하고, 실행명령(/indy/execute_plan), 정지명령(/indy/stop_robot), 목표 관절 상태(/indy/query_joint_states), MoveIt 계획결과(/move_group/result)를 구독(subscribe)합니다.

rospy의 Publisher와 Subscriber의 사용법은 다음과 같습니다.

1
2
Publisher('메시지 이름', 메시지 데이터 , queue_size=쌓을 큐의 크기)
Subscriber('메시지 이름', 메시지 데이터 , 메시지를 받을 때마다 실행할 함수 (콜백 함수), queue_size=쌓을 큐의 크기)

Subscriber에 콜백을 등록하면 메시지를 구독 할때마다 해당 함수가 실행됩니다. indy_driver에서는 다음과 같이 메시지를 받아서 필요한 정보를 joint_pos_list에 저장하는 콜백 함수, 모션 실행을 위해 플래그의 값을 변경하는 콜백함수, 로봇에 정지 명령을 보내는 콜백함수를 작성했습니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
def joint_state_callback(self, msg):
    self.joint_pos_list = [msg.state.joint_state.position]

def execute_plan_callback(self, msg):
    if msg.data is True and self.is_executed is False:
        self.is_executed = True

def plan_result_callback(self, msg):
    # download planned path from ros moveit
    self.joint_pos_list = []
    self.joint_pos_list = [p.positions for p in msg.result.planned_trajectory.joint_trajectory.points]

def stop_robot_callback(self, msg):
    if msg.data is True:
        self.indy.stop_motion()
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
def move_robot(self):
    prog = indy_program_maker.JsonProgramComponent(policy=0, resume_time=2)

    for pos in self.joint_pos_list:
        prog.add_joint_move_to(rads2degs(pos), vel=1, blend=5)

    json_string = json.dumps(prog.json_program)
    self.indy.set_and_start_json_program(json_string)
    self.joint_pos_list = []

def publish_joint_state(self):
    joint_state_msg = JointState()
    joint_state_msg.header.stamp = rospy.Time.now()

    if self.robot_name == 'NRMK-IndyRP2':
        joint_state_msg.name = ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
    else:
        joint_state_msg.name = ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5']

    joint_state_msg.position = degs2rads(self.indy.get_joint_pos())
    joint_state_msg.velocity = []
    joint_state_msg.effort = []

    self.joint_state_pub.publish(joint_state_msg)

def publish_task_pos(self):
    task_pos_msg = Float64MultiArray()
    task_pos_msg.data = self.indy.get_task_pos()
    self.task_state_pub.publish(task_pos_msg)


def run(self):
    while not rospy.is_shutdown():
        if self.is_executed:
            self.move_robot()
            self.is_executed = False
        else:
            self.publish_joint_state()
            self.publish_task_pos()
        self.rate.sleep()

작성한 클래스의 매인이 되는 run 함수에서는 ROS가 실행되고있는동안 주기적으로 publish_joint_state를 실행합니다. publish_joint_state 에서는 indy.get_joint_pos() 를 통해 받아온 로봇의 현재 관절위치를 ROS의 데이터 형식에 맞추어 저장한 다음 데이터를 발신합니다.

go_robot_callBack을 통해 is_executed의 값이 True가 되었을때 move_robot를 실행하여 저장된 joint_pos_list 위치로 로봇을 이동시킵니다. move_robot에서는 joint_pos_list의 모든 요소를 Python Program Maker 를 이용하여 로봇의 웨이포인트로 등록합니다.

간단한 ROS어플리케이션 작성(CUI)

패키지 구성

1
2
3
4
5
6
7
├── indy7_dcp_example
    ├── launch/
        └── indy7.launch
    ├── rviz_config/
    ├── src/
        └── app.py
    └── ...

본 예제에 사용하는 파일은 indy-ros-example/indy7_dcp_example폴더에 포함되어있습니다.

  • app.py파일은 사용자의 joint position 입력을 받아 해당 위치로 로봇을 이동시킵니다.

app.py

아래에 app.py 중 일부를 발췌했습니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
class JointControllerApp():
    def __init__(self):
        rospy.init_node("joint_controller")

        self.query_joint_state_pub = rospy.Publisher("/indy/query_joint_states", DisplayRobotState, queue_size=10)
        self.go_pub = rospy.Publisher("/indy/execute_plan", Bool, queue_size=1)
        self.stop_pub = rospy.Publisher("/indy/stop_robot", Bool, queue_size=1)

    def update_joint_pos(self, q):
        display_robot_state_msg = DisplayRobotState()
        display_robot_state_msg.state.joint_state.header.stamp = rospy.Time.now()
        display_robot_state_msg.state.joint_state.name = ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5']
        display_robot_state_msg.state.joint_state.position = degs2rads(q)
        display_robot_state_msg.state.joint_state.velocity = []
        display_robot_state_msg.state.joint_state.effort = []
        self.query_joint_state_pub.publish(display_robot_state_msg)

    def go(self):
        self.go_pub.publish(True)

    def stop(self):
        self.stop_pub.publish(True)

if __name__ == "__main__":
    app = JointControllerApp()

    while True:
        q = [0, 0, 0, 0, 0, 0]
        for i in range(6):
            q[i] = float(raw_input('query joint {} position '.format(i)))

        print('joint query : ' + str(q))
        app.update_joint_pos(q)

        go_sign = raw_input('enter g to move robot ')
        if go_sign == 'g':
            app.go()
        else:
            continue

        stop_sign = raw_input('enter s to move robot ')
        if stop_sign == 's':
            app.stop()
        else:
            continue

        exit_sign = raw_input('enter q to quit program ')
        if exit_sign == 'q':
            break
        else:
            continue

JointControllerApp은 ROS node를 정의하는 클래스입니다.

1
2
3
4
5
6
def __init__(self):
    rospy.init_node("joint_controller")

    self.query_joint_state_pub = rospy.Publisher("/indy/query_joint_states", DisplayRobotState, queue_size=10)
    self.go_pub = rospy.Publisher("/indy/execute_plan", Bool, queue_size=1)
    self.stop_pub = rospy.Publisher("/indy/stop_robot", Bool, queue_size=1)

line 2 에서 'joint_controller'라는 이름의 노드를 생성합니다. 해당 노드는 '/indy/query_joint_states'에 사용자가 입력한 joint position을 발신합니다. 또한 '/indy/execute_plan' 과 "/indy/stop_robot" 에 각각 신호를 발신하여 목표 위치까지 로봇을 움직이거나 정지시킵니다.

app.py 를 실행시키면 0번 관절(베이스쪽)부터 5번 관절(엔드툴쪽)까지의 관절위치를 순서대로 입력 받습니다. 입력이 완료된 후 'g'를 입력하면 로봇이 해당 위치로 이동합니다. 움직이는 도중 's'를 입력하면 로봇이 정지합니다.