layout: post title: ROS2 Simple Service Receiver on Python category: ROS2 tag: [ROS, Python] —

ROS2 Simple Service Receiver on Python

간단한 ROS2 Service를 수신하고, 커스텀 메시지 배열을 Publish 하는 예제입니다.

from simple_service.srv import SimpleService
from simple_service.msg import Position
from simple_service.msg import RoutingData

import rclpy

global g_publisher


def add_position(positions, x, y, theta):
    pos = Position()
    pos.x = x;
    pos.y = y;
    pos.theta = theta
    positions.append(pos)


def callback(request, response):
    request_id = request.request_id
    print("Simple Service Request: {0} ".format(request_id))

    if request_id == Navigation.Request.REQUEST_DEMO_MOVE_PATH_1:
        msg = RoutingData()
        msg.command = RoutingData.COMMAND_MOVE_ALONG_PATH

        add_position(msg.positions, 100, 100, 10)
        add_position(msg.positions, 500, 200, 10)
        add_position(msg.positions, 1000, 1000, 1)

        g_publisher.publish(msg)

    elif request_id == Navigation.Request.REQUEST_DEMO_MOVE_PATH_2:
        msg = RoutingData()
        msg.command = RoutingData.COMMAND_MOVE_ALONG_PATH

        add_position(msg.positions, -100, -100, 10)
        add_position(msg.positions, -500, -200, 10)
        add_position(msg.positions, -1000, -1000, 1)

        g_publisher.publish(msg)

    response.response = 200

    return response


def main(args=None):
    global g_publisher

    rclpy.init(args=args)

    node = rclpy.create_node('simple_service_receiver')
    g_publisher = node.create_publisher(RoutingData, RoutingData.NAME)

    srv = node.create_service(SimpleService, SimpleService.Request.NAME, callback)

    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

ROS2 Simple Keyboard Publisher on Python

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
  <name>simple_publisher</name>
  <version>0.4.0</version>
  <description>Simple Publisher on Python</description>
  <maintainer email="snowdeer0314@gmail.com">snowdeer</maintainer>
  <license>Apache License 2.0</license>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>simple_message</exec_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

setup.cfg

[develop]
script-dir=$base/bin/simple_publisher
[install]
install-scripts=$base/bin/simple_publisher

setup.py

from setuptools import setup

package_name = 'simple_publisher'

setup(
    name=package_name,
    version='0.4.0',
    packages=[],
    py_modules=[
        'simple_publisher'],
    install_requires=['setuptools'],
    author='snowdeer ',
    author_email='snowdeer0314@gmail.com',
    maintainer='snowdeer',
    maintainer_email='snowdeer0314@gmail.com',
    keywords=['ROS'],
    description='Simple Publisher on Python',
    license='Apache License, Version 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'simple_publisher = simple_publisher:main'
        ],
    },
)

simple_publisher.py

import threading
import rclpy
import os

from simple_message.msg import SimpleMessage

NODE_NAME = "simple_publisher"


def handle_keyboard(publisher):
    while True:
        print('\n- Simple Publisher Menu -')
        print('   1. Command (Move along path 1)')
        print('   2. Command (Move along path 2)')
        print('   99. Exit')

        menu = input('Input the menu: ')

        if menu == '1':
            msg = SimpleMessage()
            msg.command_id = SimpleMessage.COMMAND_FOR_DEMO_1
            publisher.publish(msg)
            print(" \n>>> command is published : {0}".format(msg.command_id))

        elif menu == '2':
            msg = SimpleMessage()
            msg.command_id = SimpleMessage.COMMAND_FOR_DEMO_2
            publisher.publish(msg)
            print(" \n>>> command is published : {0}".format(msg.command_id))

        elif menu == '99':
            rclpy.shutdown()
            os._exit(1)


def main(args=None):
    rclpy.init(args=args)

    node = rclpy.create_node(NODE_NAME)
    publisher = node.create_publisher(SimpleMessage, SimpleMessage.NAME)

    th = threading.Thread(target=handle_keyboard, args=(publisher,))
    th.start()

    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

람다식

람다(Lambda)는 C++11부터 지원하기 시작했으며, 이름없는 익명 함수라는 뜻을 갖고 있습니다. 재사용성이 떨어지고 한 번 쓰고 버릴 용도의 함수가 필요한 경우, 그 때마다 함수를 매번 정의해서 작성하는 것은 번거롭습니다. 이런 경우 람다 함수를 사용하면 함수의 정의없이 간편하게 함수를 사용할 수 있습니다.

함수를 변수에 지정하는 예제 코드

#include <iostream>

using namespace std;

int add(int a, int b) {
    return a + b;
}

int main() {
    cout << "hello, snowdeer!" << endl;

    auto lambda = add;

    cout << lambda(3, 4) << endl;

    return 0;
}

함수 내부에 람다식 선언

#include &ltiostream>

using namespace std;

int add(int a, int b) {
    return a + b;
}

int main() {
    cout << "hello, snowdeer!" << endl;

    auto func = [](int a, int b) -> void {
        cout << a << " + " << b << " = " << add(a, b) << endl;
    };

    func(5, 6);

    auto lambda = add;

    cout << lambda(3, 4) << endl;

    return 0;
}

위에서 보듯이 [](int a, intb) -> void 부분이 람다식입니다. 매개변수가 int a, int b이며 리턴 타입이 void인 람다 함수를 선언했습니다.

본격적인 람다 함수 사용 예제

#include <iostream>
#include <functional>
#include <memory>

using namespace std;

class LambdaTest {
public:
    LambdaTest(function<int(int, int)> lambda) : mLambdaFunc(lambda) {}

    int launch(int a, int b) {
        return mLambdaFunc(a, b);
    }

private:
    function<int(int, int)> mLambdaFunc;
};

int main() {
    cout << "hello, snowdeer!" << endl;

    shared_ptr<LambdaTest> test1 = make_shared<LambdaTest>([](int a, int b) -> int {
        return a + b;
    });

    shared_ptr<LambdaTest> test2 = make_shared<LambdaTest>([](int a, int b) -> int {
        return a - b;
    });

    shared_ptr<LambdaTest> test3 = make_shared<LambdaTest>([](int a, int b) -> int {
        return a * b;
    });

    cout << test1->launch(3, 4) << endl;
    cout << test2->launch(10, 5) << endl;
    cout << test3->launch(2, 7) << endl;

    return 0;
}

위의 예제에서 LambdaTest라는 클래스는 하나만 선언했지만, 람다식을 이용해서 그 내용을 동적으로 정의하여 다양한 용도로 사용할 수 있는 것을 볼 수 있습니다.

함수 템플릿

함수를 파라메터별로 다중으로 정의하는 경우는 편의성과 확장성을 얻을 수 있기 때문입니다. 다만, 파라메터별로 일일이 함수를 정의하는 것은 코드 길이도 길어지고 유지보수 측면에서도 좋지않은 방법입니다. 함수 템플릿을 이용하면 이를 보다 쉽고 효율적으로 사용할 수 있습니다.

예제 코드

#include <iostream>

using namespace std;

template<typename T>
T add(T a, T b) {
    return a + b;
}

int main() {
    cout << "hello, snowdeer!" << endl;
    cout << add(3, 4) << endl;
    cout << add(1.1, 2.2) << endl;
    //cout << add(string("abc"), string("def")) << endl;

    return 0;
}

함수 템플릿은 사용하는 사람이 어떤 인자를 이용해서 함수를 호출하는가에 따라 컴파일러가 다중 정의 코드를 생성합니다.

연락처 정보 가져오기

안드로이드내의 연락처 정보를 리스트 형태로 가져오는 코드입니다.

AndroidManifest.xml

AndroidManifest.xml 파일에 다음 권한을 추가합니다.

<uses-permission android:name="android.permission.READ_CONTACTS" tools:remove="android:maxSdkVersion"/>

Java 코드

public class MainActivity extends AppCompatActivity {

  static final int PERMISSION_REQUEST_CODE = 100;

  @Override
  protected void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    setContentView(R.layout.activity_main);

    if (checkSelfPermission(permission.READ_CONTACTS)
        != PackageManager.PERMISSION_GRANTED) {
      String[] permissions = new String[]{permission.READ_CONTACTS};
      requestPermissions(permissions, PERMISSION_REQUEST_CODE);
    }

    findViewById(R.id.btn_get_contact_list).setOnClickListener(new OnClickListener() {
      @Override
      public void onClick(View v) {
        getContactListAsLog();
      }
    });
  }

  @Override
  public void onRequestPermissionsResult(int requestCode, String permissions[],
      int[] grantResults) {
    super.onRequestPermissionsResult(requestCode, permissions, grantResults);

    switch (requestCode) {
      case PERMISSION_REQUEST_CODE: {
        if (grantResults.length > 0
            && grantResults[0] == PackageManager.PERMISSION_GRANTED) {
          Toast.makeText(getApplicationContext(), "Permission 완료", Toast.LENGTH_SHORT).show();
        } else {
          Toast.makeText(getApplicationContext(), "Permission 실패", Toast.LENGTH_SHORT).show();
        }
        break;
      }
    }
  }

  void getContactListAsLog() {
    ArrayList<ContactInfo> list = getContactList();

    for (ContactInfo info : list) {
      Log.i("", info.toString());
    }
  }

  ArrayList<ContactInfo> getContactList() {
    ArrayList<ContactInfo> list = new ArrayList<>();

    Uri uri = Phone.CONTENT_URI;
    String[] projection = new String[]{
        Contacts._ID,
        Contacts.PHOTO_ID,
        Phone.DISPLAY_NAME,
        Phone.NUMBER,
    };

    String sortOrder = Phone.DISPLAY_NAME + " COLLATE LOCALIZED ASC";

    Cursor cursor = getContentResolver().query(uri, projection, null, null, sortOrder);
    if (cursor.moveToFirst()) {
      do {
        ContactInfo info = new ContactInfo();

        info.id = cursor.getLong(0);
        info.photoId = cursor.getLong(1);
        info.displayName = cursor.getString(2);
        info.phoneNumber = cursor.getString(3);
        list.add(info);

      } while (cursor.moveToNext());
    }

    return list;
  }

  private Bitmap getContactPicture(long contactId) {

    Bitmap bm = null;

    try {
      InputStream inputStream = ContactsContract.Contacts
          .openContactPhotoInputStream(getContentResolver(),
              ContentUris.withAppendedId(ContactsContract.Contacts.CONTENT_URI, contactId));

      if (inputStream != null) {
        bm = BitmapFactory.decodeStream(inputStream);
        inputStream.close();
      }
    } catch (IOException e) {
      e.printStackTrace();
    }

    return bm;
  }

  class ContactInfo {

    long id;
    String displayName;
    String phoneNumber;
    long photoId;

    @Override
    public String toString() {
      return "ContactInfo{" +
          "id=" + id +
          ", displayName='" + displayName + '\'' +
          ", phoneNumber='" + phoneNumber + '\'' +
          ", photoId=" + photoId +
          '}';
    }
  }
}