diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..26d33521af10bcc7fd8cea344038eaaeb78d0ef5
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml
diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b74a26361c240f632e761e0670b71210f9ef9396
--- /dev/null
+++ b/.idea/inspectionProfiles/Project_Default.xml
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000000000000000000000000000000000000..105ce2da2d6447d11dfe32bfb846c3d5b199fc99
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/lesson_06.iml b/.idea/lesson_06.iml
new file mode 100644
index 0000000000000000000000000000000000000000..17714acb677a0aeef7cc1e8422babd500ddf753e
--- /dev/null
+++ b/.idea/lesson_06.iml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b950f4220bf019baf379f11df2093014fbb864dc
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000000000000000000000000000000000000..09e98d9813b7180a03870f9805cba5a374e095a9
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000000000000000000000000000000000000..35eb1ddfbbc029bcab630581847471d7f238ec53
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..9159b9cc96d29dca1357881bf65fbaeeb39cda75
--- /dev/null
+++ b/README.md
@@ -0,0 +1,2 @@
+Несколько костыльно реализовано общение нод контроллера и регулятора и настраиваемые коэффициенты, подобраны настройки пид, наиболее устроившие меня на данный момент.
+Костыльность заключается в том, что в переменную heading из-за использования функции get_yaw_from_quaternion при перелете угла через пи начинает поступать отрицательный угол, это частично поправлено в ноде контроллера, однако серьезно влияет на поведение модели при заданных углах 180 и 270. Попробую разобраться с этим в дальнейшем.
diff --git a/config/control-config.yaml b/config/control-config.yaml
index 80bbe66d87876352396a2d3238f2f8625038b722..8d48b5c0a655ede500547d1f5ddb985508a7f9cf 100644
--- a/config/control-config.yaml
+++ b/config/control-config.yaml
@@ -1,11 +1,10 @@
-/*:
- pid:
+pid:
ros__parameters:
angular: true
clamp: 15.0
inverted: false
- kd: 0.9
+ kd: 0.1
ki: 0.0
- kp: 0.04
+ kp: 0.21
start_type_description_service: true
- use_sim_time: false
+ use_sim_time: false
\ No newline at end of file
diff --git a/launch/step_1.launch.py b/launch/homework.launch.py
similarity index 95%
rename from launch/step_1.launch.py
rename to launch/homework.launch.py
index 2908c5e31e5e82e8b15f94ba0f50ad109b5e311f..6732b6541a1a9e3502b07d070799655c2c754a11 100644
--- a/launch/step_1.launch.py
+++ b/launch/homework.launch.py
@@ -13,7 +13,7 @@ def generate_launch_description():
Node(
package='lesson_06',
namespace='waterstrider',
- executable='step_1',
+ executable='homework',
name='controller'
),
Node(
diff --git a/launch/step_0.launch.py b/launch/step_0.launch.py
index ca123df7e3156717e8c69073577139a5d1e1fb3e..5cae864a9eb0aa1bf1ad83bfb72d1ce5cfab11ac 100644
--- a/launch/step_0.launch.py
+++ b/launch/step_0.launch.py
@@ -1,13 +1,27 @@
+import os
+from ament_index_python import get_package_share_directory
+
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
+ config_file = os.path.join(get_package_share_directory('lesson_06'),
+ 'config',
+ 'control-config.yaml')
return LaunchDescription([
Node(
package='lesson_06',
- namespace='',
+ namespace='waterstrider',
executable='step_0',
- name='controller'
+ name='controller',
+ parameters=[config_file]
+ ),
+ Node(
+ package='lesson_06',
+ namespace='waterstrider',
+ executable='pid_node',
+ name='pid',
+ parameters=[config_file]
)
- ])
+ ])
\ No newline at end of file
diff --git a/launch/step_2.launch.py b/launch/step_2.launch.py
deleted file mode 100644
index aa29cca92f4acba7a26bb8e3be1167b049340f5f..0000000000000000000000000000000000000000
--- a/launch/step_2.launch.py
+++ /dev/null
@@ -1,19 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
- return LaunchDescription([
- Node(
- package='turtlesim',
- namespace='',
- executable='turtlesim_node',
- name='sim'
- ),
- Node(
- package='lesson_06',
- namespace='',
- executable='step_2',
- name='square'
- )
- ])
diff --git a/launch/step_3.launch.py b/launch/step_3.launch.py
deleted file mode 100644
index 512b8c441e6e18e367de6c6106f5e24f3edcddb7..0000000000000000000000000000000000000000
--- a/launch/step_3.launch.py
+++ /dev/null
@@ -1,19 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
- return LaunchDescription([
- Node(
- package='turtlesim',
- namespace='',
- executable='turtlesim_node',
- name='sim'
- ),
- Node(
- package='lesson_06',
- namespace='',
- executable='step_3',
- name='square'
- )
- ])
diff --git a/launch/step_4.launch.py b/launch/step_4.launch.py
deleted file mode 100644
index ea6787d779725a4a125b93c4516a56b1d79ecf11..0000000000000000000000000000000000000000
--- a/launch/step_4.launch.py
+++ /dev/null
@@ -1,19 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
- return LaunchDescription([
- Node(
- package='turtlesim',
- namespace='',
- executable='turtlesim_node',
- name='sim'
- ),
- Node(
- package='lesson_06',
- namespace='',
- executable='step_4',
- name='square'
- )
- ])
diff --git a/lesson_06/deleted.py b/lesson_06/deleted.py
new file mode 100644
index 0000000000000000000000000000000000000000..790eebf162b9574633f7558c3a43410057547438
--- /dev/null
+++ b/lesson_06/deleted.py
@@ -0,0 +1,13 @@
+
+'''
+self.heading = self.create_publisher(Float64, '/waterstrider/pid/state', 10)
+self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10)
+self.pid_result = self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_callback, 10)
+
+
+#вариант реализации коррекции ошибки heading
+if get_yaw_from_quaternion(msg.pose.orientation) >= 0 or (0.0 <= self.desired_heading <= math.pi/2):
+ heading.data = get_yaw_from_quaternion(msg.pose.orientation)
+else:
+ heading.data = 2*math.pi + get_yaw_from_quaternion(msg.pose.orientation)
+'''
diff --git a/lesson_06/homework.py b/lesson_06/homework.py
new file mode 100644
index 0000000000000000000000000000000000000000..a8cbeb2a3d8376dd3e303d3436b895d6da0859d6
--- /dev/null
+++ b/lesson_06/homework.py
@@ -0,0 +1,82 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped, Pose
+from std_msgs.msg import Float64
+from tf_transformations import euler_from_quaternion
+import math
+
+
+def angle_diff(a1, a2):
+ a = a1 - a2
+ return (a + math.pi) % (2 * math.pi) - math.pi
+
+
+def get_yaw_from_quaternion(q):
+ return euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
+
+
+def delta_time(t1, t2):
+ t = t2 - t1
+ return t
+
+
+class RurMover(Node):
+ def __init__(self):
+ super().__init__('controller')
+ self.heading = self.create_publisher(Float64, '/waterstrider/pid/state', 10)
+ self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10)
+ self.pid_result = self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_callback, 10)
+ self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10)
+ self.pose_subscriber = self.create_subscription(PoseStamped,
+ '/waterstrider/ground_truth_to_tf_waterstrider/pose',
+ self.pose_callback, 10)
+
+ self.desired_heading = - math.pi / 2
+ self.SIDE_TIME = 20.0
+ self.LINEAR_SPEED = 0.5
+ self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+
+ def pose_callback(self, msg):
+ heading = Float64()
+ if self.desired_heading <= math.pi / 2:
+ heading.data = get_yaw_from_quaternion(msg.pose.orientation)
+ elif get_yaw_from_quaternion(msg.pose.orientation) < 0:
+ heading.data = 2 * math.pi + get_yaw_from_quaternion(msg.pose.orientation)
+ self.get_logger().info('current heading: "%f"' % heading.data)
+ self.get_logger().info('angle setpoint: "%f"' % self.desired_heading)
+ desired_heading = Float64()
+ desired_heading.data = self.desired_heading
+ self.heading.publish(heading)
+ self.heading_setpoint.publish(desired_heading)
+ '''
+
+ '''
+
+ def pid_callback(self, msg):
+ pid_out = msg.data
+ # self.get_logger().info('regulator output: "%f"' % pid_out)
+ msg_pub = Twist()
+ msg_pub.linear.x = self.LINEAR_SPEED
+ msg_pub.angular.z = pid_out
+ self.publisher_twist.publish(msg_pub)
+
+ def pub_timer_callback(self):
+ self.desired_heading += math.pi / 2
+ self.desired_heading %= 2 * math.pi
+ self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ mover = RurMover()
+
+ rclpy.spin(mover)
+
+ mover.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/lesson_06/pid_node.py b/lesson_06/pid_node.py
index 6c390a5828b2978fbcf490f1b8891926e177a912..9707de5c8829c9720ad8559c2ea1686b903414d2 100644
--- a/lesson_06/pid_node.py
+++ b/lesson_06/pid_node.py
@@ -4,6 +4,7 @@ import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
from rcl_interfaces.msg import SetParametersResult
+import math
class PID(Node):
@@ -53,13 +54,13 @@ class PID(Node):
self.pub_result.publish(message)
def angular_constraint(self, e):
- return (e + 180.0) % (2 * 180.0) - 180.0
+ return (e+math.pi) % (2*math.pi)-math.pi
def update(self, state):
error = self.sp - state if not self.config['inverted'] else state - self.sp
if self.config['angular']:
error = self.angular_constraint(error)
- self.get_logger().warn(f"PID error: {error}")
+ #self.get_logger().warn(f"PID error: {error}")
p = self.config['kp'] * error
self.e_sum += self.config['ki'] * error
self.e_sum = min(self.config['clamp'], max(self.e_sum, -self.config['clamp']))
diff --git a/lesson_06/step_0.py b/lesson_06/step_0.py
index c318b1005a99a72d19ee35d92b881158bd44153b..c45b5f979f54f6d4fb897811ea7fb004991fe780 100644
--- a/lesson_06/step_0.py
+++ b/lesson_06/step_0.py
@@ -6,7 +6,7 @@ from geometry_msgs.msg import PoseStamped, Pose
from tf_transformations import euler_from_quaternion
-import math
+import math
def angle_diff(a1, a2):
a = a1-a2
@@ -57,4 +57,4 @@ def main(args=None):
rclpy.shutdown()
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
diff --git a/setup.py b/setup.py
index 1d4258459be8c7a7075d3e94d7f1b8fbea97ea06..65b50251b0be4bc335097033e8d8140920f6404d 100644
--- a/setup.py
+++ b/setup.py
@@ -30,7 +30,7 @@ setup(
entry_points={
'console_scripts': [
'step_0 = lesson_06.step_0:main',
- 'step_1 = lesson_06.step_1:main',
+ 'homework = lesson_06.homework:main',
'pid_node = lesson_06.pid_node:main',
],
},