ABOUT ME

-

Today
-
Yesterday
-
Total
-
  • 8week ROS homework : Simple node code explain
    ROS2 Study 2021. 10. 25. 22:16
    728x90
    반응형

    #1 Create Topic msg

    1. Write code

    [helloworld_publisher.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
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    69
    70
    import rclpy
     
    # Node class import. In Node classm, there are create_publisher(), create_timer(), get_logger() and so on.
    from rclpy.node import Node
    from rclpy.node import QoSProfile
     
    # msg type import
    from std_msgs.msg import String
     
    # Create class and inherit Node class
    class HelloworldPublihser(Node):
     
        # Initializer(Constructor)
        def __init__(self):
     
            # calls the Node class's constructor and make node name
            super().__init__('helloworld_publisher')
            qos_profile = QoSProfile(depth=10)
     
            # make publisher
            self.helloworld_publisher = self.create_publisher(String, 'helloworld', qos_profile)
     
            # call callback func. every 1 sec
            self.timer = self.create_timer(1self.publish_helloworld_msg)
            self.count = 0
     
        # Define callback func.
        def publish_helloworld_msg(self):
     
            # call String class and assign to msg variable
            msg = String()
     
            # Define msg data using data property which is from String class
            msg.data = 'Helloworld: {0}'.format(self.count)
     
            # Actually publish : push on Topic
            self.helloworld_publisher.publish(msg)
     
            # Print on terminal
            self.get_logger().info('Published message: {0}'.format(msg.data))
            self.count += 1
     
    def main(args=None):
     
        # rclpy initialize
        rclpy.init(args=args)
     
        # node : instance of HelloworldPublihser class
        node = HelloworldPublihser()
     
        # play the node. class. and its mean playing callback func.
        rclpy.spin(node)
     
        # close the node
        node.destroy_node()
        rclpy.shutdown()
     
     
        '''
        try:
            rclpy.spin(node)
        except KeyboardInterrupt:
            node.get_logger().info('Keyboard Interupt (SIGINT)')
        finally:
            node.destroy_node()
            rclpy.shutdown()
        '''
     
    if __name__ == '__main__':
        main()
    cs

     

    [helloworld_subscriber.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
    52
    53
    54
    55
    56
    57
    58
    59
    import rclpy
     
    # import Node class
    from rclpy.node import Node
    from rclpy.node import QoSProfile
     
    # import String msg type class
    from std_msgs.msg import String
     
    # Create HelloworldSubscriber Class, and inherit Node class
    class HelloworldSubscriber(Node):
     
        def __init__(self):
           # calls the Node class's constructor and make node name
            super().__init__('Helloworld_subscriber')
            qos_profile = QoSProfile(depth=10)
     
            # Define subscribe node
            self.Helloworld_subscriber = self.create_subscription(
                String,
                'helloworld',
                self.subscribe_topic_message,
                qos_profile)
     
        # Define callback func.
        # msg is defined in helloworld_publisher.py
        def subscribe_topic_message(self, msg):
            # print on terminal
            self.get_logger().info('Received message: {0}'.format(msg.data))
     
     
    def main(args=None):
     
        # Initailize rclpy
        rclpy.init(args=args)
     
        # Instance of HelloworldSubscriber class
        node = HelloworldSubscriber()
     
        # play the node(HelloworldSubscriber class), its mean playing callbackfunc.
        rclpy.spin(node)
     
        # close the node
        node.destroy_node()
        rclpy.shutdown()
     
     
        '''
        try:
            rclpy.spin(node)
        except KeyboardInterrupt:
            node.get_logger().info('Keyboard Interrupt (SIGINT)')
        finally:
            node.destroy_node()
            rclpy.shutdown()
        '''
     
    if __name__ == '__main__':
        main()
    cs

     

    package.xml and setup.py 은 이전 포스팅(7주차 과제)에서 처럼 dependencies파일을 추가하여 수정하여야 한다.

     

    2. bashrc

    $ code ~/.bashrc

    아래의 코드를 추가하여 환경설정파일을 매번 새로운 터미널 창을 열때마다 자동으로 불러오게끔 한다.

     

     

     

     

    3. hello robot.com

    아래 홈페이지에서 집안일을 거드는 로봇을 볼 수 있다. 

    ROS를 사용함을 알 수 있다.

    https://hello-robot.com/

     

    Hello Robot : Simply Useful Robots

    Provider of the Stretch Research Edition. Remarkable for its capability, portability. We've reimagined the mobile manipulator.

    hello-robot.com

     

     

     

    #2 Create Srv Programming

    [sum_service.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
    import rclpy
     
    # import Node class
    from rclpy.node import Node
     
    # import AddTwoInts srv type class
    from example_interfaces.srv import AddTwoInts
     
    # Define class and inherit Node class to use Node class's func.s
    class SumServiceAsync(Node):
     
        # Initializer(Constructor)
        def __init__(self):
     
            # calls the Node class's constructor and make node name
            super().__init__('sum_service')
     
            # Define srv.
            self.srv = self.create_service(AddTwoInts, 'add_two_ints'self.add_two_ints_callback)
     
        # Define callback func.
        def add_two_ints_callback(self, request, response):
     
            # Define variable
            response.sum = request.a + request.b
            self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))
     
            return response
     
     
    def main(args=None):
        rclpy.init(args=args)
     
        sum_service = SumServiceAsync()
     
     
        try:
            rclpy.spin(sum_service)
        except KeyboardInterrupt:
            sum_service.get_logger().info('This node is finished by keboard interrupt')
        finally:
            sum_service.destroy_node()
            rclpy.shutdown()
     
     
    if __name__ == '__main__':
        main()
    cs
     

    [sum_client.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
    52
    53
    54
    55
    56
    57
    58
    59
    60
    61
    62
    63
    64
    65
    66
    67
    68
    import sys
     
    import rclpy
     
    # import Node class
    from rclpy.node import Node
     
    # import srv type class
    from example_interfaces.srv import AddTwoInts
     
    # Define SumClientAsync class
    class SumClientAsync(Node):
     
        def __init__(self):
            super().__init__('sum_client_async')
     
            # Define client
            self.cli = self.create_client(AddTwoInts, 'add_two_ints')
     
            # waiting for service getting ready
            while not self.cli.wait_for_service(timeout_sec=1.0):
                self.get_logger().info(
                    'service not available, waiting again...')
     
            # req variable declare : Request func. of AddTwoInts class
            self.req = AddTwoInts.Request()
     
     
        def send_request(self):
     
            # get command line input arg for the request
            self.req.a = int(sys.argv[1])
            self.req.b = int(sys.argv[2])
     
            # make a srv request and asynchronously get the result
            self.future = self.cli.call_async(self.req)
     
    def main(args=None):
        rclpy.init(args=args)
     
        # make instance of class
        sum_client = SumClientAsync()
        sum_client.send_request()
     
        while rclpy.ok():
     
            # excute the instance
            rclpy.spin_once(sum_client)
     
            # if srv request done
            if sum_client.future.done():
     
                try:
                    # get result and assign to variable
                    response = sum_client.future.result()
                except Exception as e:
                    sum_client.get_logger().info(
                        'Service call failed %r' %(e,))
                else:
                    sum_client.get_logger().info(
                        'Result of add_two_ints: for %d + %d = %d' % (sum_client.req.a, sum_client.req.b, response.sum))
                break
     
        sum_client.destroy_node()
        rclpy.shutdown()
     
    if __name__ == '__main__':
        main()
    cs

     

    이렇게 source code를 만든 뒤에 빌드를 통해 필요한 의존성패키지를 준비하기 위해 package.xml, CMakeLists.txt파일을 수정하여야 한다.

     

    [package.xml]

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    <?xml version="1.0"?>
    <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
    <package format="3">
      <name>sum_rclpy_srvcli_pkg</name>
      <version>0.1.0</version>
      <description>ROS2 rclpy service client package for sum</description>
      <maintainer email="jh03130@gmail.com">ju</maintainer>
      <license>Apache license 2.0</license>
     
      <depend>rclpy</depend>
      <depend>example_interfaces</depend>
     
      <test_depend>ament_copyright</test_depend>
      <test_depend>ament_flake8</test_depend>
      <test_depend>ament_pep257</test_depend>
      <test_depend>python3-pytest</test_depend>
     
      <export>
        <build_type>ament_python</build_type>
      </export>
    </package>
     
    cs
     

    [setup.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
    from setuptools import setup
     
    package_name = 'sum_rclpy_srvcli_pkg'
     
    setup(
        name=package_name,
        version='0.1.0',
        packages=[package_name],
        data_files=[
            ('share/ament_index/resource_index/packages',
                ['resource/' + package_name]),
            ('share/' + package_name, ['package.xml']),
        ],
        install_requires=['setuptools'],
        zip_safe=True,
        maintainer='ju',
        maintainer_email='jh03130@gmail.com',
        description='ROS2 rclpy service client package for sum',
        license='Apache License 2.0',
        tests_require=['pytest'],
        entry_points={
            'console_scripts': [
                'sum_service = sum_rclpy_srvcli_pkg.sum_service:main',
                'sum_client = sum_rclpy_srvcli_pkg.sum_client:main',
            ],
        },
    )
     
    cs

     

    #3  Msg, srv interface making

    보통 이미 만들어져 있는 일반적인 인터페이스를 사용할 수도 있지만 사용자에 맞게 새로운 인터페이스를 만들 수 있다. 먼저 패키지를 만들고 package.xml, CMakeLists.txt파일을 만들어줘야 한다.

    1. Create pkg for interface

    $ cd ~/robot_ws/src

    $ ros2 pkg create my_interface_example --build-type ament_cmake

     

    2. Write msg, srv code

    $ cd ~/robot_ws/src/my_interface_example

    $ mkdir msg srv

    $ cd msg

    $ touch msg/Num.msg

    $ cd ~/robot_ws/src/my_interface_example/srv

    $ touch AddThreeInts.srv

    3. Revise a package.xml

    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
    <?xml version="1.0"?>
    <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
    <package format="3">
      <name>my_interface_example</name>
      <version>0.1.0</version>
      <description>Msg and srv interface example</description>
      <maintainer email="jh03130@gmail.com">ju</maintainer>
      <license>Apache 2.0</license>
     
      <buildtool_depend>ament_cmake</buildtool_depend>
     
      <build_depend>rosidl_default_generators</build_depend>
     
      <test_depend>ament_lint_auto</test_depend>
      <test_depend>ament_lint_common</test_depend>
     
      <exec_depend>rosidl_default_runtime</exec_depend>
     
      <member_of_group>rosidl_interface_packages</member_of_group>
     
      <export>
        <build_type>ament_cmake</build_type>
      </export>
    </package>
     
    cs

    added 3 codes

    <build_depend>rosidl_default_generators</build_depend>

    <exec_depend>rosidl_default_runtime</exec_depend>

    <member_of_group>rosidl_interface_packages</member_of_group>

     

    3. Revise a CMakeLists.txt

    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
    cmake_minimum_required(VERSION 3.5)
    project(my_interface_example)
     
    # Default to C99
    if(NOT CMAKE_C_STANDARD)
      set(CMAKE_C_STANDARD 99)
    endif()
     
    # Default to C++14
    if(NOT CMAKE_CXX_STANDARD)
      set(CMAKE_CXX_STANDARD 14)
    endif()
     
    if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
      add_compile_options(-Wall -Wextra -Wpedantic)
    endif()
     
    # find dependencies
    find_package(ament_cmake REQUIRED)
    # uncomment the following section in order to fill in
    # further dependencies manually.
     
     
    # added codes
    # find_package(<dependency> REQUIRED)
    # need to convert at languages
    # below code will make python file or header file to python or cpp language
    find_package(rosidl_default_generators REQUIRED)
     
    rosidl_generate_interfaces(${PROJECT_NAME}
      "msg/Num.msg"
      "srv/AddThreeInts.srv"
      )
     
    if(BUILD_TESTING)
      find_package(ament_lint_auto REQUIRED)
      # the following line skips the linter which checks for copyrights
      # uncomment the line when a copyright and license is not present in all source files
      #set(ament_cmake_copyright_FOUND TRUE)
      # the following line skips cpplint (only works in a git repo)
      # uncomment the line when this package is not in a git repo
      #set(ament_cmake_cpplint_FOUND TRUE)
      ament_lint_auto_find_test_dependencies()
    endif()
     
    ament_package()
     
    cs

    added codes : make header files automatically

    find_package(rosidl_default_generators REQUIRED)

     

    rosidl_generate_interfaces(${PROJECT_NAME}

      "msg/Num.msg"

      "srv/AddThreeInts.srv"

      )

     

    4. check header files (after build)

    cd ~/robot_ws/install/my_interface_example/include/my_interface_example/srv or msg

     

    5. check the created interface

     

    728x90
    반응형

    댓글 0

Designed by Tistory.