diff --git a/launch/homework.launch.py b/launch/homework.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..2fb06ec94e19d3947013e8bab07abf015f4c0e47 --- /dev/null +++ b/launch/homework.launch.py @@ -0,0 +1,52 @@ +import os +from ament_index_python import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def config(pkg, filename): + path = os.path.join(get_package_share_directory(pkg), 'config', filename).split('/') + print("DEADBEAF") + print(path) + for i in range(path.index("share"), path.index("install") - 1, -1): + path.pop(i) + path.insert(i, "src") + return "/".join(path) + + +def generate_launch_description(): + namespace = 'waterstrider' + config_file = os.path.join(get_package_share_directory('lesson_07'), + 'config', + 'control-config.yaml') + + return LaunchDescription([ + Node( + package='lesson_07', + namespace=namespace, + executable='pid_node', + name='pid_distance', + parameters=[config_file] + ), + Node( + package='lesson_07', + namespace=namespace, + executable='pid_node', + name='pid_heading', + parameters=[config_file] + ), + Node( + package='lesson_07', + namespace=namespace, + executable='homework', + name='controller' + ), + Node( + package='rviz2', + namespace=namespace, + executable='rviz2', + name='rviz2', + arguments=['-d' + config('lesson_07', 'waterstrider.rviz')] + ), + ]) diff --git a/lesson_07/homework.py b/lesson_07/homework.py new file mode 100644 index 0000000000000000000000000000000000000000..cb07d258d0d4bf1230875e82853462515f442978 --- /dev/null +++ b/lesson_07/homework.py @@ -0,0 +1,127 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float64 +from geometry_msgs.msg import Twist +from geometry_msgs.msg import PoseStamped, PointStamped, Pose + +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 get_distance(delta_point): + return sum(map(lambda x: x ** 2, delta_point)) ** 0.5 + + +def get_course_to_point(delta_point): + ln = get_distance(delta_point) + if ln: + dxn = delta_point[0] / ln + dyn = delta_point[1] / ln + ang = math.acos(dxn) + if dyn < 0: + ang = 2 * math.pi - ang + return ang + return 0 + + +def get_dist_course(delta_point): + dist = get_distance(delta_point) + course = get_course_to_point(delta_point) + return dist, course + + +def get_delta_point(from_point, to_point): + return [to_point[0] - from_point[0], to_point[1] - from_point[1]] + + +class RurMover(Node): + def __init__(self): + super().__init__('controller') + self.CONTROLER_PERIOD = 1 / 10 + self.DIST_THRESHOLD = 0.2 + + self.heading = 0 + self.self_point = [0.0, 0.0] + self.goal_point = [0.0, 0.0] + zero_point = Float64() + zero_point.data = 0.0 + + self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) + + self.heading_pub = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10) + self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid_heading/setpoint', 10) + self.heading_pid_output = self.create_subscription(Float64, '/waterstrider/pid_heading/output', + self.pid_heading_callback, 10) + + self.position_pub = self.create_publisher(Float64, '/waterstrider/pid_distance/state', 10) + self.position_setpoint = self.create_publisher(Float64, '/waterstrider/pid_distance/setpoint', 10) + self.position_pid_output = self.create_subscription(Float64, '/waterstrider/pid_distance/output', + self.pid_distance_callback, 10) + + self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, + 10) + self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_callback, 10) + + self.heading_setpoint.publish(zero_point) + self.timer = self.create_timer(self.CONTROLER_PERIOD, self.update) + self.result = Twist() + + def update(self): + self.publisher_twist.publish(self.result) + + def pose_callback(self, msg): + curr_heading = Float64() + desired_heading = Float64() + position_message = Float64() + dist_threshold = Float64() + + self.self_point[0] = msg.pose.position.x + self.self_point[1] = msg.pose.position.y + delta_point = get_delta_point(self.self_point, self.goal_point) + + curr_heading.data = get_yaw_from_quaternion(msg.pose.orientation) + position_message.data, desired_heading.data = get_dist_course(delta_point) + dist_threshold.data = self.DIST_THRESHOLD + + self.heading_pub.publish(curr_heading) + self.position_pub.publish(position_message) + self.heading_setpoint.publish(desired_heading) + self.position_setpoint.publish(dist_threshold) + + def goal_pose_callback(self, msg): + self.goal_point[0] = msg.pose.position.x + self.goal_point[1] = msg.pose.position.y + + def pid_distance_callback(self, msg): + self.result.linear.x = -1 * msg.data + + def pid_heading_callback(self, msg): + self.result.angular.z = -1 * msg.data + + def log_info(self, msg: str) -> None: + self.get_logger().info(msg) + + +def main(args=None): + rclpy.init(args=args) + + mover = RurMover() + + rclpy.spin(mover) + + mover.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/setup.py b/setup.py index 2cffc5aaef47797c4f3dced93341377691ac3122..331134336d666a01951629d9d0854c87be421761 100644 --- a/setup.py +++ b/setup.py @@ -32,7 +32,8 @@ setup( 'step_0 = lesson_07.step_0:main', 'step_1 = lesson_07.step_1:main', 'pid_node = lesson_07.pid_node:main', - 'move2point = lesson_07.move2point:main' + 'move2point = lesson_07.move2point:main', + 'homework = lesson_07.homework:main', ], }, )