├── my_first_package ├── resource │ └── my_first_package ├── my_first_package │ ├── __init__.py │ ├── my_first_node.py │ ├── my_multi_thread.py │ ├── my_subscriber.py │ ├── my_publisher.py │ ├── turtle_cmd_and_pose.py │ ├── my_service_server.py │ └── dist_turtle_action_server.py ├── setup.cfg ├── params │ └── turtlesim.yaml ├── launch │ ├── turtlesim_and_teleop.launch.py │ └── dist_turtle_action.launch.py ├── package.xml ├── test │ ├── test_copyright.py │ ├── test_pep257.py │ └── test_flake8.py └── setup.py ├── .DS_Store ├── img ├── book.png └── image.png ├── my_first_package_msgs ├── srv │ └── MultiSpawn.srv ├── msg │ └── CmdAndPoseVel.msg ├── action │ └── DistTurtle.action ├── package.xml └── CMakeLists.txt └── README.md /my_first_package/resource/my_first_package: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PinkWink/ros2_basic/HEAD/.DS_Store -------------------------------------------------------------------------------- /img/book.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PinkWink/ros2_basic/HEAD/img/book.png -------------------------------------------------------------------------------- /img/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PinkWink/ros2_basic/HEAD/img/image.png -------------------------------------------------------------------------------- /my_first_package_msgs/srv/MultiSpawn.srv: -------------------------------------------------------------------------------- 1 | int64 num 2 | --- 3 | float64[] x 4 | float64[] y 5 | float64[] theta -------------------------------------------------------------------------------- /my_first_package/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/my_first_package 3 | [install] 4 | install_scripts=$base/lib/my_first_package 5 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/my_first_node.py: -------------------------------------------------------------------------------- 1 | def main(): 2 | print('Hi from my_first_package.') 3 | 4 | 5 | if __name__ == '__main__': 6 | main() 7 | -------------------------------------------------------------------------------- /my_first_package_msgs/msg/CmdAndPoseVel.msg: -------------------------------------------------------------------------------- 1 | float32 cmd_vel_linear 2 | float32 cmd_vel_angular 3 | 4 | float32 pose_x 5 | float32 pose_y 6 | float32 linear_vel 7 | float32 angular_vel -------------------------------------------------------------------------------- /my_first_package_msgs/action/DistTurtle.action: -------------------------------------------------------------------------------- 1 | # Request 2 | float32 linear_x 3 | float32 angular_z 4 | float32 dist 5 | --- 6 | # Result 7 | float32 pos_x 8 | float32 pos_y 9 | float32 pos_theta 10 | float32 result_dist 11 | --- 12 | # Feedback 13 | float32 remained_dist 14 | -------------------------------------------------------------------------------- /my_first_package/params/turtlesim.yaml: -------------------------------------------------------------------------------- 1 | /turtlesim: 2 | ros__parameters: 3 | background_b: 203 4 | background_g: 192 5 | background_r: 255 6 | qos_overrides: 7 | /parameter_events: 8 | publisher: 9 | depth: 1000 10 | durability: volatile 11 | history: keep_last 12 | reliability: reliable 13 | use_sim_time: false 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS2 혼자 공부하는 로봇 SW 2 | 3 | 4 | 5 | * 코드는 책의 제일 마지막 챕터 기준입니다. 6 | * 이 코드를 바로 git clone 하지 마시고, 오타 등을 확인하는 용도로 사용하시면 좋겠습니다. 7 | 8 | ### 영상 강의 9 | * 영상 강의는 계속 업로드 중입니다. 10 | * 아래 재생목록 링크를 가지고 계시면 계속 업로드되는 상황을 확인하실 수 있습니다. 11 | 12 | https://www.youtube.com/playlist?list=PL0xYz_4oqpvhj4JaPSTeGI2k5GQEE36oi 13 | 14 | ### 정오표 15 | 16 | 17 | 18 | * 위 정오표는 독자이신 "박만규"님께서 메일로 알려주신 오류를 제시한 것입니다. 19 | * 독자 "박만규"님께 감사드리고, 또 이런 실수를 해서 여러 독자분들께 죄송합니다. -------------------------------------------------------------------------------- /my_first_package/launch/turtlesim_and_teleop.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | 4 | def generate_launch_description(): 5 | return LaunchDescription( 6 | [ 7 | Node( 8 | namespace= "turtlesim", package='turtlesim', 9 | executable='turtlesim_node', output='screen'), 10 | Node( 11 | namespace= "pub_cmd_vel", package='my_first_package', 12 | executable='my_publisher', output='screen'), 13 | ] 14 | ) -------------------------------------------------------------------------------- /my_first_package/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | my_first_package 5 | 0.0.0 6 | TODO: Package description 7 | pw 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/my_multi_thread.py: -------------------------------------------------------------------------------- 1 | import rclpy as rp 2 | from rclpy.executors import MultiThreadedExecutor 3 | from rclpy.node import Node 4 | 5 | from my_first_package.my_publisher import TurtlesimPublisher 6 | from my_first_package.my_subscriber import TurtlesimSubscriber 7 | 8 | 9 | def main(args=None): 10 | rp.init() 11 | 12 | sub = TurtlesimSubscriber() 13 | pub = TurtlesimPublisher() 14 | 15 | executor = MultiThreadedExecutor() 16 | 17 | executor.add_node(sub) 18 | executor.add_node(pub) 19 | 20 | try: 21 | executor.spin() 22 | 23 | finally: 24 | executor.shutdown() 25 | sub.destroy_node() 26 | pub.destroy_node() 27 | rp.shutdown() 28 | 29 | 30 | if __name__ == '__main__': 31 | main() -------------------------------------------------------------------------------- /my_first_package/launch/dist_turtle_action.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node, ExcuteProcess 3 | 4 | def generate_launch_description(): 5 | my_launch = LaunchDescription() 6 | 7 | turtlesim_node = Node( 8 | package='turtlesim', 9 | executable='turtlesim_node', 10 | output='screen', 11 | parameters=[ 12 | {"background_r": 255}, 13 | {"background_g": 192}, 14 | {"background_b": 203}, 15 | ] 16 | ) 17 | 18 | dist_turtle_action_node = Node( 19 | package='my_first_package', 20 | executable='dist_turtle_action_server', 21 | output='screen', 22 | ) 23 | 24 | my_launch.add_action(turtlesim_node) 25 | my_launch.add_action(dist_turtle_action_node) 26 | 27 | return my_launch 28 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/my_subscriber.py: -------------------------------------------------------------------------------- 1 | import rclpy as rp 2 | from rclpy.node import Node 3 | 4 | from turtlesim.msg import Pose 5 | 6 | 7 | class TurtlesimSubscriber(Node): 8 | 9 | def __init__(self): 10 | super().__init__('turtlesim_subscriber') 11 | self.subscription = self.create_subscription( 12 | Pose, 13 | '/turtle1/pose', 14 | self.callback, 15 | 10) 16 | self.subscription # prevent unused variable warning 17 | 18 | def callback(self, msg): 19 | print("X : ", msg.x, ", Y : ", msg.y) 20 | 21 | 22 | def main(args=None): 23 | rp.init(args=args) 24 | 25 | turtlesim_subscriber = TurtlesimSubscriber() 26 | rp.spin(turtlesim_subscriber) 27 | 28 | turtlesim_subscriber.destroy_node() 29 | rp.shutdown() 30 | 31 | 32 | if __name__ == '__main__': 33 | main() 34 | -------------------------------------------------------------------------------- /my_first_package/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /my_first_package/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /my_first_package_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | my_first_package_msgs 5 | 0.0.0 6 | TODO: Package description 7 | pw 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rosidl_default_generators 16 | rosidl_default_runtime 17 | rosidl_interface_packages 18 | 19 | action_msgs 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/my_publisher.py: -------------------------------------------------------------------------------- 1 | import rclpy as rp 2 | from rclpy.node import Node 3 | 4 | from geometry_msgs.msg import Twist 5 | 6 | import sys 7 | print(sys.argv) 8 | 9 | 10 | class TurtlesimPublisher(Node): 11 | 12 | def __init__(self): 13 | super().__init__('turtlesim_publisher') 14 | self.publisher = self.create_publisher(Twist, '/turtlesim/turtle1/cmd_vel', 10) 15 | timer_period = 0.5 16 | self.timer = self.create_timer(timer_period, self.timer_callback) 17 | 18 | def timer_callback(self): 19 | msg = Twist() 20 | msg.linear.x = 2.0 21 | msg.angular.z = 2.0 22 | self.publisher.publish(msg) 23 | 24 | 25 | def main(args=None): 26 | rp.init(args=args) 27 | 28 | turtlesim_publisher = TurtlesimPublisher() 29 | rp.spin(turtlesim_publisher) 30 | 31 | turtlesim_publisher.destroy_node() 32 | rp.shutdown() 33 | 34 | 35 | if __name__ == '__main__': 36 | main() 37 | 38 | -------------------------------------------------------------------------------- /my_first_package/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /my_first_package_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(my_first_package_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | find_package(rosidl_default_generators REQUIRED) 15 | 16 | rosidl_generate_interfaces(${PROJECT_NAME} 17 | "msg/CmdAndPoseVel.msg" 18 | "srv/MultiSpawn.srv" 19 | "action/DistTurtle.action" 20 | ) 21 | 22 | if(BUILD_TESTING) 23 | find_package(ament_lint_auto REQUIRED) 24 | # the following line skips the linter which checks for copyrights 25 | # uncomment the line when a copyright and license is not present in all source files 26 | #set(ament_cmake_copyright_FOUND TRUE) 27 | # the following line skips cpplint (only works in a git repo) 28 | # uncomment the line when this package is not in a git repo 29 | #set(ament_cmake_cpplint_FOUND TRUE) 30 | ament_lint_auto_find_test_dependencies() 31 | endif() 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /my_first_package/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import os 3 | import glob 4 | 5 | package_name = 'my_first_package' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.0', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', '*.launch.py'))), 16 | ], 17 | install_requires=['setuptools'], 18 | zip_safe=True, 19 | maintainer='pw', 20 | maintainer_email='pw@todo.todo', 21 | description='TODO: Package description', 22 | license='TODO: License declaration', 23 | tests_require=['pytest'], 24 | entry_points={ 25 | 'console_scripts': [ 26 | 'my_first_node = my_first_package.my_first_node:main', 27 | 'my_subscriber = my_first_package.my_subscriber:main', 28 | 'my_publisher = my_first_package.my_publisher:main', 29 | 'turtle_cmd_and_pose = my_first_package.turtle_cmd_and_pose:main', 30 | 'my_service_server = my_first_package.my_service_server:main', 31 | 'dist_turtle_action_server = my_first_package.dist_turtle_action_server:main', 32 | 'my_multi_thread = my_first_package.my_multi_thread:main' 33 | ], 34 | }, 35 | ) 36 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/turtle_cmd_and_pose.py: -------------------------------------------------------------------------------- 1 | import rclpy as rp 2 | from rclpy.node import Node 3 | 4 | from turtlesim.msg import Pose 5 | from geometry_msgs.msg import Twist 6 | from my_first_package_msgs.msg import CmdAndPoseVel 7 | 8 | 9 | class CmdAndPose(Node): 10 | 11 | def __init__(self): 12 | super().__init__('turtle_cmd_pose') 13 | self.sub_pose = self.create_subscription(Pose, '/turtle1/pose', self.callback_pose, 10) 14 | self.sub_cmdvel = self.create_subscription(Twist, '/turtle1/cmd_vel', self.callback_cmd, 10) 15 | self.timer_period = 1.0 16 | self.publisher = self.create_publisher(CmdAndPoseVel, '/cmd_and_pose', 10) 17 | self.timer = self.create_timer(self.timer_period, self.timer_callback) 18 | self.cmd_pose = CmdAndPoseVel() 19 | 20 | def callback_pose(self, msg): 21 | self.cmd_pose.pose_x = msg.x 22 | self.cmd_pose.pose_y = msg.y 23 | self.cmd_pose.linear_vel = msg.linear_velocity 24 | self.cmd_pose.angular_vel = msg.angular_velocity 25 | 26 | def callback_cmd(self, msg): 27 | self.cmd_pose.cmd_vel_linear = msg.linear.x 28 | self.cmd_pose.cmd_vel_angular = msg.angular.z 29 | 30 | def timer_callback(self): 31 | self.publisher.publish(self.cmd_pose) 32 | 33 | 34 | def main(args=None): 35 | rp.init(args=args) 36 | 37 | turtle_cmd_pose_node = CmdAndPose() 38 | rp.spin(turtle_cmd_pose_node) 39 | 40 | turtle_cmd_pose_node.destroy_node() 41 | rp.shutdown() 42 | 43 | 44 | if __name__ == '__main__': 45 | main() 46 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/my_service_server.py: -------------------------------------------------------------------------------- 1 | from my_first_package_msgs.srv import MultiSpawn 2 | from turtlesim.srv import TeleportAbsolute 3 | from turtlesim.srv import Spawn 4 | 5 | import rclpy as rp 6 | import numpy as np 7 | from rclpy.node import Node 8 | 9 | 10 | class MultiSpawning(Node): 11 | 12 | def __init__(self): 13 | super().__init__('multi_spawn') 14 | self.server = self.create_service(MultiSpawn, 'multi_spawn', self.callback_service) 15 | self.teleport = self.create_client(TeleportAbsolute, '/turtle1/teleport_absolute') 16 | self.spawn = self.create_client(Spawn, '/spawn') 17 | self.req_teleport = TeleportAbsolute.Request() 18 | self.req_spawn = Spawn.Request() 19 | self.center_x = 5.54 20 | self.center_y = 5.54 21 | 22 | 23 | def calc_position(self, n, r): 24 | gap_theta = 2*np.pi / n 25 | theta = [gap_theta*n for n in range(n)] 26 | x = [r*np.cos(th) for th in theta] 27 | y = [r*np.sin(th) for th in theta] 28 | 29 | return x, y, theta 30 | 31 | def callback_service(self, request, response): 32 | x, y, theta = self.calc_position(request.num, 3) 33 | 34 | for n in range(len(theta)): 35 | self.req_spawn.x = x[n] + self.center_x 36 | self.req_spawn.y = y[n] + self.center_y 37 | self.req_spawn.theta = theta[n] 38 | self.spawn.call_async(self.req_spawn) 39 | 40 | response.x = x 41 | response.y = y 42 | response.theta = theta 43 | 44 | return response 45 | 46 | def main(args=None): 47 | rp.init(args=args) 48 | multi_spawn = MultiSpawning() 49 | rp.spin(multi_spawn) 50 | rp.shutdown() 51 | 52 | if __name__ == '__main__': 53 | main() 54 | -------------------------------------------------------------------------------- /my_first_package/my_first_package/dist_turtle_action_server.py: -------------------------------------------------------------------------------- 1 | import rclpy as rp 2 | from rclpy.action import ActionServer 3 | from rclpy.executors import MultiThreadedExecutor 4 | from rclpy.node import Node 5 | 6 | from turtlesim.msg import Pose 7 | from geometry_msgs.msg import Twist 8 | from my_first_package_msgs.action import DistTurtle 9 | from my_first_package.my_subscriber import TurtlesimSubscriber 10 | 11 | from rcl_interfaces.msg import SetParametersResult 12 | 13 | import math 14 | import time 15 | 16 | class TurtleSub_Action(TurtlesimSubscriber): 17 | def __init__(self, ac_server): 18 | super().__init__() 19 | self.ac_server = ac_server 20 | 21 | def callback(self, msg): 22 | self.ac_server.current_pose = msg 23 | 24 | class DistTurtleServer(Node): 25 | def __init__(self): 26 | super().__init__('dist_turtle_action_server') 27 | self.total_dist = 0 28 | self.is_first_time = True 29 | self.current_pose = Pose() 30 | self.previous_pose = Pose() 31 | self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10) 32 | self.action_server = ActionServer(self, DistTurtle, 'dist_turtle', self.excute_callback) 33 | 34 | self.get_logger().info('Dist turtle action server is started.') 35 | 36 | self.declare_parameter('quatile_time', 0.75) 37 | self.declare_parameter('almost_goal_time', 0.95) 38 | 39 | (quantile_time, almosts_time) = self.get_parameters( 40 | ['quatile_time', 'almost_goal_time']) 41 | self.quantile_time = quantile_time.value 42 | self.almosts_time = almosts_time.value 43 | 44 | output_msg = "quantile_time is " + str(self.quantile_time) + ". " 45 | output_msg = output_msg + "and almost_goal_time is " + str(self.almosts_time) + ". " 46 | self.get_logger().info(output_msg) 47 | 48 | self.add_on_set_parameters_callback(self.parameter_callback) 49 | 50 | def parameter_callback(self, params): 51 | for param in params: 52 | print(param.name, " is changed to ", param.value) 53 | 54 | if param.name == 'quatile_time': 55 | self.quantile_time = param.value 56 | if param.name == 'almost_goal_time': 57 | self.almosts_time = param.value 58 | 59 | output_msg = "quantile_time is " + str(self.quantile_time) + ". " 60 | output_msg = output_msg + "and almost_goal_time is " + str(self.almosts_time) + ". " 61 | self.get_logger().info(output_msg) 62 | 63 | return SetParametersResult(successful=True) 64 | 65 | def calc_diff_pose(self): 66 | if self.is_first_time: 67 | self.previous_pose.x = self.current_pose.x 68 | self.previous_pose.y = self.current_pose.y 69 | self.is_first_time = False 70 | 71 | diff_dist = math.sqrt((self.current_pose.x - self.previous_pose.x)**2 +\ 72 | (self.current_pose.y - self.previous_pose.y)**2) 73 | 74 | self.previous_pose = self.current_pose 75 | 76 | return diff_dist 77 | 78 | def excute_callback(self, goal_handle): 79 | feedback_msg = DistTurtle.Feedback() 80 | 81 | msg = Twist() 82 | msg.linear.x = goal_handle.request.linear_x 83 | msg.angular.z = goal_handle.request.angular_z 84 | 85 | while True: 86 | self.total_dist += self.calc_diff_pose() 87 | feedback_msg.remained_dist = goal_handle.request.dist - self.total_dist 88 | goal_handle.publish_feedback(feedback_msg) 89 | self.publisher.publish(msg) 90 | 91 | tmp = feedback_msg.remained_dist - goal_handle.request.dist * self.quantile_time 92 | tmp = abs(tmp) 93 | 94 | if tmp < 0.02: 95 | output_msg = 'The turtle passes the ' + str(self.quantile_time) + ' point. ' 96 | output_msg = output_msg + ' : ' + str(tmp) 97 | self.get_logger().info(output_msg) 98 | 99 | time.sleep(0.01) 100 | 101 | if feedback_msg.remained_dist < 0.2: 102 | break 103 | 104 | goal_handle.succeed() 105 | result = DistTurtle.Result() 106 | 107 | result.pos_x = self.current_pose.x 108 | result.pos_y = self.current_pose.y 109 | result.pos_theta = self.current_pose.theta 110 | result.result_dist = self.total_dist 111 | 112 | self.total_dist = 0 113 | self.is_first_time = True 114 | 115 | return result 116 | 117 | 118 | def main(args=None): 119 | rp.init(args=args) 120 | 121 | executor = MultiThreadedExecutor() 122 | 123 | ac = DistTurtleServer() 124 | sub = TurtleSub_Action(ac_server = ac) 125 | 126 | executor.add_node(sub) 127 | executor.add_node(ac) 128 | 129 | try: 130 | executor.spin() 131 | 132 | finally: 133 | executor.shutdown() 134 | sub.destroy_node() 135 | ac.destroy_node() 136 | rp.shutdown() 137 | 138 | 139 | if __name__ == '__main__': 140 | main() 141 | 142 | --------------------------------------------------------------------------------