융합_로보테크 AI 자율주행 로봇 개발자 과정-26/03/17

파라미터 마무리

/home/steezer/ros2_study/src/my_first_package/my_first_package/dist_turtle_action_server.py

import rclpy as rp
from rclpy.action import ActionServer
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_first_package_msgs.action import DistTurtle
from my_first_package.my_subscriber import TurtlesimSubscriber

from rcl_interfaces.msg import SetParametersResult

import math
import time

class TurtleSub_Action(TurtlesimSubscriber):
    def __init__(self, ac_server):
        super().__init__()
        self.ac_server = ac_server

    def callback(self, msg):
        self.ac_server.current_pose = msg

class DistTurtleServer(Node):
    def __init__(self):
        super().__init__('dist_turtle_action_server')
        self.total_dist = 0
        self.is_first_time = True
        self.current_pose = Pose()
        self.previous_pose = Pose()
        self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self._action_server = ActionServer(self, DistTurtle, 'dist_turtle', self.execute_callback)
        self.declare_parameter('quatile_time', 0.75)
        self.declare_parameter('almost_goal_time', 0.95)

        (quantile_time, almosts_time) = self.get_parameters(['quatile_time', 'almost_goal_time'])
        self.quantile_time = quantile_time.value
        self.almosts_time = almosts_time.value

        self.add_on_set_parameters_callback(self.parameter_callback)

    def parameter_callback(self, params):
        for param in params:
            print(param.name, " is changed to ", param.value)

            if param.name == 'quatile_time':
                self.quantile_time = param.value
            if param.name == 'almost_goal_time':
                self.almosts_time = param.value

        print('quatile_time and almost_goal_time is ', self.quantile_time, self.almosts_time)

        return SetParametersResult(successful=True)

    def calc_diff_pose(self):
        if self.is_first_time:
            self.previous_pose.x = self.current_pose.x
            self.previous_pose.y = self.current_pose.y
            self.is_first_time = False

        diff_dist = math.sqrt((self.current_pose.x - self.previous_pose.x)**2 +\
        					  (self.current_pose.y - self.previous_pose.y)**2)

        self.previous_pose = self.current_pose

        return diff_dist

    def execute_callback(self, goal_handle):
        feedback_msg = DistTurtle.Feedback()

        msg = Twist()
        msg.linear.x = goal_handle.request.linear_x
        msg.angular.z = goal_handle.request.angular_z

        while True:
            self.total_dist += self.calc_diff_pose()
            feedback_msg.remained_dist = goal_handle.request.dist - self.total_dist
            goal_handle.publish_feedback(feedback_msg)
            self.publisher.publish(msg)
            time.sleep(0.01)

            if feedback_msg.remained_dist < 0.2:
                break

        goal_handle.succeed()
        result = DistTurtle.Result()

        result.pos_x = self.current_pose.x
        result.pos_y = self.current_pose.y
        result.pos_theta = self.current_pose.theta
        result.result_dist = self.total_dist

        self.total_dist = 0
        self.is_first_time = True

        return result


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

    executor = MultiThreadedExecutor()

    ac = DistTurtleServer()
    sub = TurtleSub_Action(ac_server = ac)

    executor.add_node(sub)
    executor.add_node(ac)

    try:
        executor.spin()

    finally:
        executor.shutdown()
        sub.destroy_node()
        ac.destroy_node()
        rp.shutdown()


if __name__ == '__main__':
    main()

ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ cd ~/ros2_study
steezer@DESKTOP-TF8J569:~/ros2_study$ source install/setup.bash
steezer@DESKTOP-TF8J569:~/ros2_study$ ros2 param set /dist_turtle_action_server quatile_time 0.8
Set parameter successful
steezer@DESKTOP-TF8J569:~/ros2_study$ ros2 param set /dist_turtle_action_server almost_goal_time 0.99
Set parameter successful

ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ cd ~/ros2_study
steezer@DESKTOP-TF8J569:~/ros2_study$ source install/setup.bash
steezer@DESKTOP-TF8J569:~/ros2_study$ ros2 run my_first_package dist_turtle_action_server
quatile_time  is changed to  0.8
quatile_time and almost_goal_time is  0.8 0.95
almost_goal_time  is changed to  0.99
quatile_time and almost_goal_time is  0.8 0.99

 

 

11장 디버그와 관찰을 위한 여러 도구들

 

print()함수는 내용을 콘솔에 출력하기 위해 사용

print() 함수로 출력한 내용은 해당 터미널을 끄면 사라짐

ros가 지원하는 기록까지 가능한 출력 함수 사용 ->logger

 

rqt

메시지 통신 관련 CRUD 가능한 인터페이스 제공하는 그래픽 기반 프로그램

 

ros2 topic pub -r 1 /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2., y: 0., z: 0.}, angular: {x: 0., y: 0., z: 0.}}"

 

Fatal

치명적 문제

Error

실행하다 코드에서 예외 발생

Warn

프로그램 실행에는 문제 없지만 확인할 것

Info

상태 모니터링

Debug

수정하기 위해 테스트 중인 부분

 

topic monitor

topic publisher

steezer@DESKTOP-TF8J569:~/ros2_study$ rqt --force-discover

import rclpy as rp
from rclpy.action import ActionServer
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_first_package_msgs.action import DistTurtle
from my_first_package.my_subscriber import TurtlesimSubscriber

from rcl_interfaces.msg import SetParametersResult

import math
import time

class TurtleSub_Action(TurtlesimSubscriber):
    def __init__(self, ac_server):
        super().__init__()
        self.ac_server = ac_server

    def callback(self, msg):
        self.ac_server.current_pose = msg

class DistTurtleServer(Node):
    def __init__(self):
        super().__init__('dist_turtle_action_server')
        self.total_dist = 0
        self.is_first_time = True
        self.current_pose = Pose()
        self.previous_pose = Pose()
        self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.action_server = ActionServer(self, DistTurtle, 'dist_turtle', self.excute_callback)

        self.get_logger().info('Dist turtle action server is started.')

        self.declare_parameter('quatile_time', 0.75)
        self.declare_parameter('almost_goal_time', 0.95)

        (quantile_time, almosts_time) = self.get_parameters(
                                            ['quatile_time', 'almost_goal_time'])
        self.quantile_time = quantile_time.value
        self.almosts_time = almosts_time.value

        output_msg = "quantile_time is " + str(self.quantile_time) + ". "
        output_msg = output_msg + "and almost_goal_time is " + str(self.almosts_time) + ". "
        self.get_logger().info(output_msg)

        self.add_on_set_parameters_callback(self.parameter_callback)

    def parameter_callback(self, params):
        for param in params:
            print(param.name, " is changed to ", param.value)

            if param.name == 'quatile_time':
                self.quantile_time = param.value
            if param.name == 'almost_goal_time':
                self.almosts_time = param.value

        output_msg = "quantile_time is " + str(self.quantile_time) + ". "
        output_msg = output_msg + "and almost_goal_time is " + str(self.almosts_time) + ". "
        self.get_logger().info(output_msg)

        return SetParametersResult(successful=True)

    def calc_diff_pose(self):
        if self.is_first_time:
            self.previous_pose.x = self.current_pose.x
            self.previous_pose.y = self.current_pose.y
            self.is_first_time = False

        diff_dist = math.sqrt((self.current_pose.x - self.previous_pose.x)**2 +\
                             (self.current_pose.y - self.previous_pose.y)**2)

        self.previous_pose = self.current_pose

        return diff_dist

    def excute_callback(self, goal_handle):
        feedback_msg = DistTurtle.Feedback()

        msg = Twist()
        msg.linear.x = goal_handle.request.linear_x
        msg.angular.z = goal_handle.request.angular_z

        while True:
            self.total_dist += self.calc_diff_pose()
            feedback_msg.remained_dist = goal_handle.request.dist - self.total_dist
            goal_handle.publish_feedback(feedback_msg)
            self.publisher.publish(msg)

            tmp = feedback_msg.remained_dist - goal_handle.request.dist * self.quantile_time
            tmp = abs(tmp)

            if tmp < 0.02:
                output_msg = 'The turtle passes the ' + str(self.quantile_time) + ' point. '
                output_msg = output_msg + ' : ' + str(tmp)
                self.get_logger().info(output_msg)

            time.sleep(0.01)

            if feedback_msg.remained_dist < 0.2:
                break

        goal_handle.succeed()
        result = DistTurtle.Result()

        result.pos_x = self.current_pose.x
        result.pos_y = self.current_pose.y
        result.pos_theta = self.current_pose.theta
        result.result_dist = self.total_dist

        self.total_dist = 0
        self.is_first_time = True

        return result


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

    executor = MultiThreadedExecutor()

    ac = DistTurtleServer()
    sub = TurtleSub_Action(ac_server = ac)

    executor.add_node(sub)
    executor.add_node(ac)

    try:
        executor.spin()

    finally:
        executor.shutdown()
        sub.destroy_node()
        ac.destroy_node()
        rp.shutdown()


if __name__ == '__main__':
    main()

 

 

roslaunch

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription(
        [
            Node(
                namespace= "turtlesim", package='turtlesim',
                executable= "turtlesim_node", output='screen'),
            Node(
                namespace= "pub_cmd_vel", package='my_first_package',
                executable='my_publisher', output='screen'),
        ]
    )

from setuptools import find_packages, setup
import os
import glob


package_name = 'my_first_package'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', '*launch.py'))),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='steezer',
    maintainer_email='steezer@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    extras_require={
        'test': [
            'pytest',
        ],
    },
    entry_points={
        'console_scripts': [
            'my_first_node = my_first_package.my_first_node:main',
            'my_subscriber = my_first_package.my_subscriber:main',
            'my_publisher = my_first_package.my_publisher:main',
            'turtle_cmd_and_pose = my_first_package.turtle_cmd_and_pose:main',
            'my_service_server = my_first_package.my_service_server:main',
            'dist_turtle_action_server = my_first_package.dist_turtle_action_server:main',
            'my_multi_thread = my_first_package.my_multi_thread:main'
        ],
    },
)

import rclpy as rp
from rclpy.node import Node

from geometry_msgs.msg import Twist


class TurtlesimPublisher(Node):

	def __init__(self):
		super().__init__('turtlesim_publisher')
		self.publisher = self.create_publisher(Twist, '/turtlesim/turtle1/cmd_vel', 10)
		timer_period = 1.0
		self.timer = self.create_timer(timer_period, self.timer_callback)

	def timer_callback(self):
		msg = Twist()
		msg.linear.x = 2.0
		msg.angular.z = 2.0
		self.publisher.publish(msg)


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

	turtlesim_publisher = TurtlesimPublisher()
	rp.spin(turtlesim_publisher)

	turtlesim_publisher.destroy_node()
	rp.shutdown()


if __name__ == '__main__':
	main()


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

	turtlesim_publisher = TurtlesimPublisher()
	rp.spin(turtlesim_publisher) # Ctrl + C to exit

	turtlesim_publisher.destroy_node()
	rp.shutdown()


if __name__ == '__main__':
	main()

steezer@DESKTOP-TF8J569:~$ ros2 launch my_first_package turtlesim_and_teleop.launch.py
[INFO] [launch]: All log files can be found below /home/steezer/.ros/log/2026-03-17-11-44-03-281086-DESKTOP-TF8J569-7733
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [7734]
[INFO] [my_publisher-2]: process started with pid [7736]
[turtlesim_node-1] [INFO] [1773715445.414147577] [turtlesim.turtlesim]: Starting turtlesim with node name /turtlesim/turtlesim
[turtlesim_node-1] [INFO] [1773715445.422975251] [turtlesim.turtlesim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]

 

/home/steezer/ros2_study/src/my_first_package/launch/dist_turtle_action.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    my_launch = LaunchDescription()

    turtlesim_node = Node(
        package='turtlesim', 
        executable='turtlesim_node', 
        output='screen',
        parameters=[
            {"background_r": 227},
            {"background_g": 227},
            {"background_b": 227},
        ]
    )

    dist_turtle_action_node = Node(
        package='my_first_package', 
        executable='dist_turtle_action_server', 
        output='screen',
    )

    my_launch.add_action(turtlesim_node)
    my_launch.add_action(dist_turtle_action_node)

    return my_launch

/home/steezer/ros2_study/src/my_first_package/launch/dist_turtle_action.launch.py

steezer@DESKTOP-TF8J569:~/ros2_study$ ros2 launch my_first_package dist_turtle_action.launch.py
[INFO] [launch]: All log files can be found below /home/steezer/.ros/log/2026-03-17-11-50-28-054545-DESKTOP-TF8J569-8458
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [turtlesim_node-1]: process started with pid [8459]
[INFO] [dist_turtle_action_server-2]: process started with pid [8461]
[turtlesim_node-1] [INFO] [1773715828.498197198] [turtlesim]: Starting turtlesim with node name /turtlesim
[turtlesim_node-1] [INFO] [1773715828.506063123] [turtlesim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]
[dist_turtle_action_server-2] [INFO] [1773715828.540159399] [dist_turtle_action_server]: Dist turtle action server is started.
[dist_turtle_action_server-2] [INFO] [1773715828.541536686] [dist_turtle_action_server]: quantile_time is 0.75. and almost_goal_time is 0.95.

---

ROS2 입문 마무리 과제(개인)

1. 새로운 워크스페이스 생성한다.
2. 그 안에 새로운 패키지를 생성한다(패키지명은 my_second_package).
3. git을 이용한 버전 관리를 진행하면서 작업한다.
4. 작업 내용은 위에 적힌대로, dist_turtle_action_server.py를 C++ 버전으로 집필한다.
5. 중간중간 git commit하는 타이밍은 개인이 판단하여 작업한다.
6. 완성되면, github에 저장소 생성하여 업로드까지 하여 마무리한다.

규칙

1. 8교시 전까진 구글링과 책으로 해결한다.
2. 해결 안되는 문제로 고민하는 시간은 스스로 제한한다.
3. LLM 서비스는 8교시 이후로 사용한다.

ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ ls
260309  260310  260311  greetings  normal_file  python  ros2_study  snap
steezer@DESKTOP-TF8J569:~$ mkdir -p my_ws/src
steezer@DESKTOP-TF8J569:~$ cd my_ws
steezer@DESKTOP-TF8J569:~/my_ws$ ros2 pkg create --build-type ament_cmake --node-name dist_turtle_action_server my_second_package
going to create a new package
package name: my_second_package
destination directory: /home/steezer/my_ws
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['steezer <steezer@todo.todo>']
licenses: ['TODO: License declaration']
build type: ament_cmake
dependencies: []
node_name: dist_turtle_action_server
creating folder ./my_second_package
creating ./my_second_package/package.xml
creating source and include folder
creating folder ./my_second_package/src
creating folder ./my_second_package/include/my_second_package
creating ./my_second_package/CMakeLists.txt
creating ./my_second_package/src/dist_turtle_action_server.cpp

[WARNING]: Unknown license 'TODO: License declaration'.  This has been set in the package.xml, but no LICENSE file has been created.
It is recommended to use one of the ament license identitifers:
Apache-2.0
BSL-1.0
BSD-2.0
BSD-2-Clause
BSD-3-Clause
GPL-3.0-only
LGPL-3.0-only
MIT
MIT-0
steezer@DESKTOP-TF8J569:~/my_ws$ git --version
git version 2.34.1
steezer@DESKTOP-TF8J569:~/my_ws$ mv my_second_package src
steezer@DESKTOP-TF8J569:~/my_ws$ ls
src
steezer@DESKTOP-TF8J569:~/my_ws$ git init
hint: Using 'master' as the name for the initial branch. This default branch name
hint: is subject to change. To configure the initial branch name to use in all
hint: of your new repositories, which will suppress this warning, call:
hint:
hint:   git config --global init.defaultBranch <name>
hint:
hint: Names commonly chosen instead of 'master' are 'main', 'trunk' and
hint: 'development'. The just-created branch can be renamed via this command:
hint:
hint:   git branch -m <name>
Initialized empty Git repository in /home/steezer/my_ws/.git/
steezer@DESKTOP-TF8J569:~/my_ws$ ls -a
.  ..  .git  src
steezer@DESKTOP-TF8J569:~/my_ws$ touch .gitignore
steezer@DESKTOP-TF8J569:~/my_ws$ subl .

 

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_second_package</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="steezer@todo.todo">steezer</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>rclcpp_action</depend>
  <depend>turtlesim</depend>
  <depend>geometry_msgs</depend>
  <depend>my_first_package_msgs</depend>
  <depend>rcl_interfaces</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

 

+github 업로드

 

CmakeLists.txt 수정

cmake_minimum_required(VERSION 3.8)
project(my_second_package)

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.
# find_package(<dependency> REQUIRED)

find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(turtlesim REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(my_first_package_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)

add_executable(dist_turtle_action_server
  src/dist_turtle_action_server.cpp
)

ament_target_dependencies(dist_turtle_action_server
  rclcpp
  rclcpp_action
  turtlesim
  geometry_msgs
  my_first_package_msgs
  rcl_interfaces
)

install(TARGETS
  dist_turtle_action_server
  DESTINATION lib/${PROJECT_NAME}
)

add_executable(dist_turtle_action_server src/dist_turtle_action_server.cpp)
target_include_directories(dist_turtle_action_server PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(dist_turtle_action_server PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

install(TARGETS dist_turtle_action_server
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

 

steezer@DESKTOP-TF8J569:~/my_ws$ colcon build --cmake-args \
  -DCMAKE_PREFIX_PATH=/home/steezer/ros2_study/install/my_first_package_msgs
Starting >>> my_second_package
Finished <<< my_second_package [12.5s]

Summary: 1 package finished [12.8s]

steezer@DESKTOP-TF8J569:~/my_ws$ colcon build --cmake-args \
  -DCMAKE_PREFIX_PATH=/home/steezer/ros2_study/install/my_first_package_msgs
Starting >>> my_second_package
Finished <<< my_second_package [12.5s]

Summary: 1 package finished [12.8s]
steezer@DESKTOP-TF8J569:~/my_ws$ source ~/.bashrc
ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~/my_ws$ ros2 run turtlesim turtlesim_node
[INFO] [1773727488.856581196] [turtlesim]: Starting turtlesim with node name /turtlesim
[INFO] [1773727488.866992934] [turtlesim]: Spawning turtle [turtle1] at x=[5.544445], y=[5.544445], theta=[0.000000]

ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ source ~/.bashrc
ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ source ~/my_ws/install/setup.bash
steezer@DESKTOP-TF8J569:~$ ros2 run my_second_package dist_turtle_action_server
[INFO] [1773727513.656778261] [dist_turtle_action_server]: quantile_time: 0.75, almost_goal_time: 0.95
[INFO] [1773727513.656940560] [dist_turtle_action_server]: Dist turtle action server is started.
[INFO] [1773727536.287988205] [dist_turtle_action_server]: The turtle passes the 0.75 point : 0.0020
[INFO] [1773727536.299259181] [dist_turtle_action_server]: The turtle passes the 0.75 point : 0.0020

ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ source ~/.bashrc
ROS2 humble is activated!
steezer@DESKTOP-TF8J569:~$ ros2 action send_goal /dist_turtle my_first_package_msgs/action/DistTurtle \
"{'dist': 5.0, 'linear_x': 2.0, 'angular_z': 0.0}"
Waiting for an action server to become available...
Sending goal:
     linear_x: 2.0
angular_z: 0.0
dist: 5.0

Goal accepted with ID: c03e68d5b7bb43838b080a24c2751c25

Result:
    pos_x: 10.376444816589355
pos_y: 5.544444561004639
pos_theta: 0.0
result_dist: 4.832000255584717

Goal finished with status: SUCCEEDED

 

steezer@DESKTOP-TF8J569:~/my_ws$ git add src/my_second_package/CMakeLists.txt
steezer@DESKTOP-TF8J569:~/my_ws$ git commit -m "build: fix duplicate target in CMakeLists.txt"
[master 0811977] build: fix duplicate target in CMakeLists.txt
 1 file changed, 7 insertions(+), 20 deletions(-)
steezer@DESKTOP-TF8J569:~/my_ws$ git push origin master
Username for 'https://github.com': steeze1213
Password for 'https://steeze1213@github.com':
Enumerating objects: 9, done.
Counting objects: 100% (9/9), done.
Delta compression using up to 8 threads
Compressing objects: 100% (4/4), done.
Writing objects: 100% (5/5), 534 bytes | 534.00 KiB/s, done.
Total 5 (delta 1), reused 0 (delta 0), pack-reused 0
remote: Resolving deltas: 100% (1/1), completed with 1 local object.
To https://github.com/steeze1213/my_ws.git
   956eaa3..0811977  master -> master

 

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <turtlesim/msg/pose.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <my_first_package_msgs/action/dist_turtle.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <cmath>
#include <thread>
#include <chrono>

using DistTurtle = my_first_package_msgs::action::DistTurtle;
using GoalHandle = rclcpp_action::ServerGoalHandle<DistTurtle>;

class DistTurtleServer : public rclcpp::Node
{
public:
    DistTurtleServer() : Node("dist_turtle_action_server")
    {
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/turtle1/cmd_vel", 10);

        pose_sub_ = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose", 10,
            [this](const turtlesim::msg::Pose::SharedPtr msg) {
                current_pose_ = *msg;
            });

        action_server_ = rclcpp_action::create_server<DistTurtle>(
            this, "dist_turtle",
            [this](const rclcpp_action::GoalUUID &,
                   std::shared_ptr<const DistTurtle::Goal>) {
                return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
            },
            [this](const std::shared_ptr<GoalHandle>) {
                return rclcpp_action::CancelResponse::ACCEPT;
            },
            [this](const std::shared_ptr<GoalHandle> goal_handle) {
                std::thread([this, goal_handle]() {
                    execute_callback(goal_handle);
                }).detach();
            });

        this->declare_parameter("quatile_time", 0.75);
        this->declare_parameter("almost_goal_time", 0.95);
        quantile_time_ = this->get_parameter("quatile_time").as_double();
        almost_goal_time_ = this->get_parameter("almost_goal_time").as_double();

        RCLCPP_INFO(this->get_logger(),
            "quantile_time: %.2f, almost_goal_time: %.2f",
            quantile_time_, almost_goal_time_);

        param_cb_ = this->add_on_set_parameters_callback(
            [this](const std::vector<rclcpp::Parameter> &params) {
                for (const auto &p : params) {
                    RCLCPP_INFO(this->get_logger(),
                        "%s is changed to %.2f",
                        p.get_name().c_str(), p.as_double());
                    if (p.get_name() == "quatile_time")
                        quantile_time_ = p.as_double();
                    else if (p.get_name() == "almost_goal_time")
                        almost_goal_time_ = p.as_double();
                }
                rcl_interfaces::msg::SetParametersResult result;
                result.successful = true;
                return result;
            });

        RCLCPP_INFO(this->get_logger(), "Dist turtle action server is started.");
    }

private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
    rclcpp_action::Server<DistTurtle>::SharedPtr action_server_;
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;

    turtlesim::msg::Pose current_pose_;
    turtlesim::msg::Pose previous_pose_;
    double total_dist_ = 0.0;
    bool is_first_time_ = true;
    double quantile_time_ = 0.75;
    double almost_goal_time_ = 0.95;

    double calc_diff_pose()
    {
        if (is_first_time_) {
            previous_pose_.x = current_pose_.x;
            previous_pose_.y = current_pose_.y;
            is_first_time_ = false;
        }
        double dx = current_pose_.x - previous_pose_.x;
        double dy = current_pose_.y - previous_pose_.y;
        double diff = std::sqrt(dx * dx + dy * dy);
        previous_pose_ = current_pose_;
        return diff;
    }

    void execute_callback(const std::shared_ptr<GoalHandle> goal_handle)
    {
        total_dist_ = 0.0;
        is_first_time_ = true;

        auto feedback = std::make_shared<DistTurtle::Feedback>();
        geometry_msgs::msg::Twist msg;
        msg.linear.x = goal_handle->get_goal()->linear_x;
        msg.angular.z = goal_handle->get_goal()->angular_z;

        while (true) {
            total_dist_ += calc_diff_pose();
            feedback->remained_dist =
                goal_handle->get_goal()->dist - total_dist_;
            goal_handle->publish_feedback(feedback);
            publisher_->publish(msg);

            double tmp = std::abs(
                feedback->remained_dist -
                goal_handle->get_goal()->dist * quantile_time_);
            if (tmp < 0.02) {
                RCLCPP_INFO(this->get_logger(),
                    "The turtle passes the %.2f point : %.4f",
                    quantile_time_, tmp);
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(10));

            if (feedback->remained_dist < 0.2)
                break;
        }

        auto result = std::make_shared<DistTurtle::Result>();
        result->pos_x = current_pose_.x;
        result->pos_y = current_pose_.y;
        result->pos_theta = current_pose_.theta;
        result->result_dist = total_dist_;
        goal_handle->succeed(result);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DistTurtleServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}