8week ROS homework : Simple node code explain

#1 Create Topic msg

1. Write code

[helloworld_publisher.py]

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(1, self.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()

 

[helloworld_subscriber.py]

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()

 

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

 

 

 

#2 Create Srv Programming

[sum_service.py]

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()

 

[sum_client.py]

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()

 

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

 

[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="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>
 

 

[setup.py]

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',
        ],
    },
)
 

#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

<?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>
 

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

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()
 

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