Attached arm to rover, arm moving script added

.gitignore

@@ -0,0 +1,3 @@
+build/
+install/
+log/

src/arm_harverster/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED)
 # Installs your Python script file and marks it executable
 install(PROGRAMS
   scripts/move_to_point.py
+  scripts/arm_point_controller.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/arm_harverster/scripts/arm_point_controller.py

@@ -0,0 +1,235 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+import math
+import time
+
+# ROS 2 Message, Action, and Service Imports
+from moveit_msgs.action import MoveGroup, ExecuteTrajectory
+from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume, RobotState
+from moveit_msgs.srv import GetCartesianPath, GetPositionFK
+from shape_msgs.msg import SolidPrimitive
+from geometry_msgs.msg import PoseStamped, Pose
+from sensor_msgs.msg import JointState
+
+class CameraArmController(Node):
+    def __init__(self):
+        super().__init__('camera_arm_controller')
+        
+        # 1. Action Client for normal point-to-point planning
+        self._action_client = ActionClient(self, MoveGroup, '/move_action')
+        self.get_logger().info("Waiting for MoveIt action server...")
+        self._action_client.wait_for_server()
+        
+        # 2. Action Client for running raw pre-calculated paths
+        self._execute_client = ActionClient(self, ExecuteTrajectory, '/execute_trajectory')
+        self.get_logger().info("Waiting for execution action server...")
+        self._execute_client.wait_for_server()
+        
+        # 3. Service Client for calculating straight horizontal camera paths
+        self._cartesian_client = self.create_client(GetCartesianPath, '/compute_cartesian_path')
+        self.get_logger().info("Connecting to Cartesian path service...")
+        self._cartesian_client.wait_for_service()
+
+        # 4. NEW: Service Client for fetching live end-effector coordinates (Forward Kinematics)
+        self._fk_client = self.create_client(GetPositionFK, '/compute_fk')
+        self.get_logger().info("Connecting to Forward Kinematics service...")
+        self._fk_client.wait_for_service()
+        
+        # 5. Subscriber to listen to current joint states (Required to compute current position)
+        self.current_joint_state = None
+        self._joint_sub = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_state_callback,
+            10
+        )
+        
+        self.get_logger().info("🚀 Automated Camera Arm Pipeline Fully Operational!")
+
+    def joint_state_callback(self, msg):
+        """Saves current joint angles to calculate position on the fly"""
+        self.current_joint_state = msg
+
+    def get_horizontal_camera_quaternion(self):
+        """Calculates a 90-degree pitch offset to keep the camera level with the horizon"""
+        pitch_angle = math.radians(-90.0)
+        q = Pose().orientation
+        q.x = 0.0
+        q.y = math.sin(pitch_angle / 2.0)
+        q.z = 0.0
+        q.w = math.cos(pitch_angle / 2.0)
+        return q
+
+    def get_current_end_effector_pose(self):
+        """Queries the FK service to find exactly where the camera is right now"""
+        # Wait until we receive at least one joint state message from the robot
+        while self.current_joint_state is None and rclpy.ok():
+            self.get_logger().info("Waiting for initial joint states...")
+            rclpy.spin_once(self, timeout_sec=0.1)
+
+        req = GetPositionFK.Request()
+        req.header.frame_id = "world"
+        req.fk_link_names = ["tool_link"] # Name of the tip frame to track
+        
+        # Pack current joint configuration into the request
+        robot_state = RobotState()
+        robot_state.joint_state = self.current_joint_state
+        req.robot_state = robot_state
+
+        # Call the service synchronously
+        future = self._fk_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        
+        res = future.result()
+        if res and res.error_code.val == 1: # 1 = SUCCESS
+            return res.pose_stamped[0].pose
+        else:
+            self.get_logger().error("Failed to calculate Forward Kinematics!")
+            return None
+
+    def send_pt_to_pt_goal(self, x, y, z):
+        """Moves the camera to a specific 3D coordinate while forcing it to be horizontal"""
+        self.get_logger().info(f"Planning Point-to-Point Move to: X={x}, Y={y}, Z={z}")
+        
+        goal_msg = MoveGroup.Goal()
+        goal_msg.request.group_name = "arm"
+        goal_msg.request.allowed_planning_time = 10.0
+        goal_msg.request.num_planning_attempts = 15
+        goal_msg.request.pipeline_id = "ompl"
+        
+        target_pose = PoseStamped()
+        target_pose.header.frame_id = "world"
+        target_pose.pose.position.x = float(x)
+        target_pose.pose.position.y = float(y)
+        target_pose.pose.position.z = float(z)
+        target_pose.pose.orientation = self.get_horizontal_camera_quaternion()
+
+        position_constraint = PositionConstraint()
+        position_constraint.header.frame_id = "world"
+        position_constraint.link_name = "tool_link"
+        
+        box = SolidPrimitive()
+        box.type = SolidPrimitive.BOX
+        box.dimensions = [0.03, 0.03, 0.03]
+        
+        volume = BoundingVolume()
+        volume.primitives.append(box)
+        
+        box_pose = Pose()
+        box_pose.position = target_pose.pose.position
+        box_pose.orientation.w = 1.0
+        volume.primitive_poses.append(box_pose)
+        
+        position_constraint.constraint_region = volume
+        position_constraint.weight = 1.0
+
+        orient_constraint = OrientationConstraint()
+        orient_constraint.header.frame_id = "world"
+        orient_constraint.link_name = "tool_link"
+        orient_constraint.orientation = target_pose.pose.orientation
+        
+        orient_constraint.absolute_x_axis_tolerance = 0.15  
+        orient_constraint.absolute_y_axis_tolerance = 0.15  
+        orient_constraint.absolute_z_axis_tolerance = 3.14  
+        orient_constraint.weight = 1.0
+
+        goal_constraints = Constraints()
+        goal_constraints.position_constraints.append(position_constraint)
+        goal_constraints.orientation_constraints.append(orient_constraint)
+        goal_msg.request.goal_constraints.append(goal_constraints)
+
+        send_goal_future = self._action_client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        
+        goal_handle = send_goal_future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error("❌ Point-to-Point goal rejected by MoveIt!")
+            return False
+
+        self.get_logger().info("Goal accepted! Executing path...")
+        get_result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self, get_result_future)
+        return True
+
+    def execute_straight_camera_sweep(self, distance=0.2):
+        """
+        AUTOMATED NO-XYZ METHOD: Fetches live position from the simulation 
+        and advances the camera straight forward parallel to the ground.
+        """
+        # 1. Fetch live position automatically
+        current_pose = self.get_current_end_effector_pose()
+        if current_pose is None:
+            self.get_logger().error("Cannot track sweep because current pose is unknown.")
+            return False
+
+        current_x = current_pose.position.x
+        current_y = current_pose.position.y
+        current_z = current_pose.position.z
+
+        self.get_logger().info(f"📍 Detected current position: X={current_x:.3f}, Y={current_y:.3f}, Z={current_z:.3f}")
+        self.get_logger().info(f"Calculating straight crawl: advancing +{distance}m forward along X...")
+        
+        req = GetCartesianPath.Request()
+        req.group_name = "arm"
+        req.header.frame_id = "world"
+        req.start_state.is_diff = True 
+        
+        # Target waypoint calculated using live variables
+        target_waypoint = Pose()
+        target_waypoint.position.x = current_x + distance
+        target_waypoint.position.y = current_y
+        target_waypoint.position.z = current_z
+        target_waypoint.orientation = self.get_horizontal_camera_quaternion() 
+
+        req.waypoints = [target_waypoint]
+        req.max_step = 0.01          
+        req.jump_threshold = 0.0     
+
+        # Call Cartesian planner service
+        future = self._cartesian_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        
+        res = future.result()
+        if res.fraction < 1.0:
+            self.get_logger().warn(f"⚠️ Only calculated {res.fraction*100:.1f}% of path due to link extension limits.")
+            if res.fraction < 0.5:
+                return False
+
+        self.get_logger().info("Executing calculated straight line Cartesian trajectory...")
+        execute_goal = ExecuteTrajectory.Goal()
+        execute_goal.trajectory = res.solution 
+
+        send_goal_future = self._execute_client.send_goal_async(execute_goal)
+        rclpy.spin_until_future_complete(self, send_goal_future)
+        
+        goal_handle = send_goal_future.result()
+        if goal_handle and goal_handle.accepted:
+            get_result_future = goal_handle.get_result_async()
+            rclpy.spin_until_future_complete(self, get_result_future)
+            self.get_logger().info("✅ Straight camera sweep complete!")
+            return True
+        else:
+            self.get_logger().error("❌ Trajectory execution rejected.")
+            return False
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CameraArmController()
+
+    # 1. Send to starting exploration zone
+    success = node.send_pt_to_pt_goal(0.6, 0.0, 0.5)
+    
+    if success:
+        time.sleep(2.0) 
+        
+        # 2. Call the sweep without passing any XYZ parameters! 
+        # It handles the math completely internally.
+        node.execute_straight_camera_sweep(distance=0.2)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/arm_harverster/scripts/move_to_point.py

@@ -29,10 +29,14 @@ class ArmController(Node):
         # 2. Define our target pose
         target_pose = PoseStamped()
         target_pose.header.frame_id = "world"
-        target_pose.pose.position.x = 0.2
+        target_pose.pose.position.x = -0.3
-        target_pose.pose.position.y = 0.1
+        target_pose.pose.position.y = 1.0
-        target_pose.pose.position.z = 0.4
+        target_pose.pose.position.z = 0.9
-        target_pose.pose.orientation.w = 1.0  # Facing neutral forward
+
+        target_pose.pose.orientation.x = 0.0
+        target_pose.pose.orientation.y = 0.0
+        target_pose.pose.orientation.z = 0.0
+        target_pose.pose.orientation.w = 1.0
 
         # 3. Package the target pose into MoveIt's expected constraints format
         position_constraint = PositionConstraint()

src/arm_moveit_config/.setup_assistant

@@ -7,19 +7,4 @@ moveit_setup_assistant_config:
   package_settings:
     author_name: marcin
     author_email: 113255@example.com
-    generated_timestamp: 1780594876
+    generated_timestamp: 1781026475
-  control_xacro:
-    command:
-      - position
-    state:
-      - position
-      - velocity
-  modified_urdf:
-    xacros:
-      - control_xacro
-  control_xacro:
-    command:
-      - position
-    state:
-      - position
-      - velocity

src/arm_moveit_config/config/joint_limits.yaml

@@ -12,29 +12,29 @@ joint_limits:
     has_velocity_limits: true
     max_velocity: 2.5
     has_acceleration_limits: true
-    max_acceleration: 5.0
+    max_acceleration: 10.0
   joint_2:
     has_velocity_limits: true
     max_velocity: 2.0
     has_acceleration_limits: true
-    max_acceleration: 5.0
+    max_acceleration: 10.0
   joint_3:
     has_velocity_limits: true
     max_velocity: 2.5
     has_acceleration_limits: true
-    max_acceleration: 5.0
+    max_acceleration: 10.0
   joint_4:
     has_velocity_limits: true
     max_velocity: 3.0
     has_acceleration_limits: true
-    max_acceleration: 5.0
+    max_acceleration: 10.0
   joint_5:
     has_velocity_limits: true
     max_velocity: 3.0
     has_acceleration_limits: true
-    max_acceleration: 5.0
+    max_acceleration: 10.0
   joint_6:
     has_velocity_limits: true
     max_velocity: 4.0
     has_acceleration_limits: true
-    max_acceleration: 5.0
+    max_acceleration: 10.0

src/arm_moveit_config/config/kinematics.yaml

@@ -1,4 +1,5 @@
 arm:
-  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
-  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.0050000000000000001
+  kinematics_solver_timeout: 0.05
+  solve_type: Distance # Tells TRAC-IK to minimize deviations smoothly

src/arm_moveit_config/config/moveit_controllers.yaml

@@ -8,8 +8,6 @@ moveit_simple_controller_manager:
 
   arm_controller:
     type: FollowJointTrajectory
-    action_ns: follow_joint_trajectory
-    default: True
     joints:
       - joint_1
       - joint_2
@@ -17,3 +15,5 @@ moveit_simple_controller_manager:
       - joint_4
       - joint_5
       - joint_6
+    action_ns: follow_joint_trajectory
+    default: True

src/arm_moveit_config/config/ros2_controllers.yaml

@@ -20,8 +20,7 @@ arm_controller:
       - joint_5
       - joint_6
     command_interfaces:
-      - position
+      []
     state_interfaces:
-      - position
+      []
-      - velocity
     allow_nonzero_velocity_at_trajectory_end: true

src/arm_moveit_config/config/sensors_3d.yaml

@@ -0,0 +1,23 @@
+sensors:
+  - kinect_pointcloud
+  - kinect_depthimage
+kinect_pointcloud:
+  filtered_cloud_topic: filtered_cloud
+  max_range: 5.0
+  max_update_rate: 1.0
+  padding_offset: 0.1
+  padding_scale: 1.0
+  point_cloud_topic: /head_mount_kinect/depth_registered/points
+  point_subsample: 1
+  sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+kinect_depthimage:
+  far_clipping_plane_distance: 5.0
+  filtered_cloud_topic: filtered_cloud
+  image_topic: /head_mount_kinect/depth_registered/image_raw
+  max_update_rate: 1.0
+  near_clipping_plane_distance: 0.3
+  padding_offset: 0.03
+  padding_scale: 4.0
+  queue_size: 5
+  sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+  shadow_threshold: 0.2

src/arm_moveit_config/config/six_axis_arm.srdf

@@ -15,11 +15,13 @@
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="tip" parent_link="tool_link" group="arm"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="world" type="fixed" parent_frame="world" child_link="base_link"/>
+    <virtual_joint name="mobile_base_attachment" type="fixed" parent_frame="mounting_plate" child_link="base_link" />
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
     <disable_collisions link1="base_link" link2="link_2" reason="Never"/>
+    <disable_collisions link1="base_link" link2="link_3" reason="Never"/>
     <disable_collisions link1="link_1" link2="link_2" reason="Adjacent"/>
+    <disable_collisions link1="link_1" link2="link_3" reason="Never"/>
     <disable_collisions link1="link_2" link2="link_3" reason="Adjacent"/>
     <disable_collisions link1="link_2" link2="link_4" reason="Never"/>
     <disable_collisions link1="link_2" link2="link_5" reason="Never"/>

src/arm_moveit_config/launch/move_group.launch.py

@@ -3,6 +3,13 @@ from moveit_configs_utils.launches import generate_move_group_launch
 
 
 def generate_launch_description():
-    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    #moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    moveit_config = (
+        MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config")
+        # Overrides the parameters dictionary layout natively
+        .to_moveit_configs()
+    )
+    
+    # Manually append use_sim_time to the master parameter dictionaries
     moveit_config.robot_description_kinematics["use_sim_time"] = True
     return generate_move_group_launch(moveit_config)

src/arm_moveit_config/launch/sim.launch.py

@@ -1,74 +1,94 @@
 import os
+import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler, TimerAction
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
-from launch.actions import IncludeLaunchDescription, TimerAction
+
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+# Absolute and relative path variables
+world_path = './src/world_description/corn_field.sdf'
+arm_urdf_path = '/home/marcin/arm_ws/robotarm_description/urdf/robot_arm.urdf'
+rover_sdf_path = '/home/marcin/arm_ws/src/world_description/harvester.sdf'
 
 def generate_launch_description():
-    # 1. Define package directories (Adjust names if yours differ)
     moveit_config_pkg = get_package_share_directory('arm_moveit_config')
     
-    # 2. Include the standard MoveIt 2 move_group launch file
+    # 1. Include the standard MoveIt 2 move_group launch file (Delayed for stability)
-    # This automatically loads the robot description (URDF/XACRO) and handles robot_state_publisher
     move_group_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(moveit_config_pkg, 'launch', 'move_group.launch.py')
         )
     )
 
-    # 3. Launch Gazebo Sim (Modern Gazebo) empty world
+    # 2. Launch Gazebo Sim (Modern Gazebo) loaded directly into your field world
     gazebo_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
         ),
-        launch_arguments={'gz_args': '-r empty.sdf'}.items(), # '-r' runs simulation immediately
+        launch_arguments={'gz_args': '-r ' + world_path }.items(),
     )
 
-    # 4. Spawn the robot entity inside the Gazebo world
+    # 3. MODIFIED: Spawns the ENTIRE rover model from the master SDF file.
-    # It listens to the /robot_description topic published by MoveIt's robot_state_publisher
+    # Because your SDF includes the arm internally, Gazebo builds the combined asset together.
-    spawn_robot = Node(
+    spawn_complete_rover = Node(
         package='ros_gz_sim',
         executable='create',
         arguments=[
-            '-topic', 'robot_description',
+            '-file', rover_sdf_path,
-            '-name', 'six_axis_arm',
+            '-name', 'harvester_robot', # Matches the model namespace inside your world bridges
-            '-z', '0.0' # Adjust if your base needs to sit higher
+            '-z', '0.1'                 # Clear the field ground mesh on entry
         ],
         output='screen',
     )
 
-    # 5. Define ros2_control Spawners
+    # 4. Define ros2_control Spawners
-    # Broadcaster for joint states (vital for MoveIt to know where the arm currently is)
     joint_state_broadcaster_spawner = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
     )
 
-    # Trajectory controller for sending trajectory paths to Gazebo
     arm_controller_spawner = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        arguments=['arm_controller', '--controller-manager', '/controller_manager'],
+        arguments=["arm_controller", "--controller-manager", "/controller_manager"],
     )
 
-    # 6. Bridge Gazebo Clock to ROS 2 (Crucial for simulation time syncing)
+    # 5. Bridge Gazebo Clock and Data Channels to ROS 2 Jazzy
     bridge = Node(
         package='ros_gz_bridge',
         executable='parameter_bridge',
-        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        arguments=[
+            '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
+            '/world/corn_field_wide/model/harvester_robot/joint_state@sensor_msgs/msg/JointState[gz.msgs.Model',
+            '/model/harvester_robot/joint/joint_1/cmd_pos@std_msgs/msg/Float64]gz.msgs.Double'
+            ],
         output='screen'
     )
 
-    # 7. Delay controller spawning until AFTER the robot is spawned in Gazebo
+    # 6. Process the xacro file dynamically for the ROS 2 Kinematics Stack
-    # This prevents ros2_control from crashing due to Gazebo not being ready yet
+    xacro_file = os.path.join(
+        get_package_share_directory('robotarm_description'),
+        'urdf',
+        'robot_arm.urdf.xacro'
+    )
+    robot_description_raw = xacro.process_file(xacro_file).toxml()
+
+    # 7. Robot State Publisher (Feeds the mathematical link tree strictly to MoveIt 2)
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='both',
+        parameters=[{'robot_description': robot_description_raw, 'use_sim_time': True}]
+    )
+
+    # 8. Timing/Order Sequence Event Handlers
+    # Waits for the COMPLETE rover to exist before spinning up hardware controller instances
     delay_joint_state_broadcaster = RegisterEventHandler(
         event_handler=OnProcessExit(
-            target_action=spawn_robot,
+            target_action=spawn_complete_rover,
             on_exit=[joint_state_broadcaster_spawner],
         )
     )
@@ -79,41 +99,18 @@ def generate_launch_description():
             on_exit=[arm_controller_spawner],
         )
     )
+    
     delay_move_group_launch = TimerAction(
         period=5.0,
         actions=[move_group_launch]
     )
 
-    import xacro
-
-    # ... inside generate_launch_description() ...
-
-    # 1. Path to your actual Xacro file
-    xacro_file = os.path.join(
-        get_package_share_directory('robotarm_description'), # Change to your description pkg
-        'urdf',
-        'robot_arm.urdf.xacro' # Change to your actual file name
-    )
-
-    # 2. Process the xacro file into raw XML text
-    robot_description_raw = xacro.process_file(xacro_file).toxml()
-
-    # 3. Add the explicit Robot State Publisher Node
-    robot_state_publisher_node = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        output='screen',
-        parameters=[{'robot_description': robot_description_raw, 'use_sim_time': True}]
-    )
-
-
     return LaunchDescription([
         gazebo_launch,
-        delay_move_group_launch,
+        spawn_complete_rover, # Spawns the single, complete vehicle
-        #move_group_launch,
+        robot_state_publisher,
-        spawn_robot,
         bridge,
         delay_joint_state_broadcaster,
         delay_arm_controller,
-        robot_state_publisher_node,
+        delay_move_group_launch,
     ])

src/arm_moveit_config/package.xml

@@ -43,7 +43,6 @@
   <exec_depend>rviz_default_plugins</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>warehouse_ros_mongo</exec_depend>
-  <exec_depend>xacro</exec_depend>
 
 
   <export>

src/robotarm_description/urdf/robot_arm.urdf

@@ -0,0 +1,269 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from robot_arm.urdf.xacro           | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="six_axis_arm">
+  <material name="grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.42 0.04 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.05 0.35 0.65 1.0"/>
+  </material>
+  <!-- <link name="world"/>
+
+  <joint name="world_to_base" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint> -->
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.05"/>
+      <geometry>
+        <cylinder length="0.1" radius="0.15"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.05"/>
+      <geometry>
+        <cylinder length="0.1" radius="0.15"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.05"/>
+      <mass value="5.0"/>
+      <inertia ixx="0.032291666666666656" ixy="0.0" ixz="0.0" iyy="0.032291666666666656" iyz="0.0" izz="0.056249999999999994"/>
+    </inertial>
+  </link>
+  <joint name="joint_1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <origin rpy="0 0 0" xyz="0 0 0.1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="300.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.24"/>
+      <geometry>
+        <cylinder length="0.48" radius="0.08"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.24"/>
+      <geometry>
+        <cylinder length="0.48" radius="0.08"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.24"/>
+      <mass value="4.0"/>
+      <inertia ixx="0.0832" ixy="0.0" ixz="0.0" iyy="0.0832" iyz="0.0" izz="0.0128"/>
+    </inertial>
+  </link>
+  <joint name="joint_2" type="revolute">
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <origin rpy="0 0 0" xyz="0 0 0.48"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="300.0" lower="-2.0" upper="2.0" velocity="2.0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link_2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.32000000000000006"/>
+      <geometry>
+        <cylinder length="0.6400000000000001" radius="0.07"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.32000000000000006"/>
+      <geometry>
+        <cylinder length="0.6400000000000001" radius="0.07"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.32000000000000006"/>
+      <mass value="3.5"/>
+      <inertia ixx="0.1237541666666667" ixy="0.0" ixz="0.0" iyy="0.1237541666666667" iyz="0.0" izz="0.008575000000000001"/>
+    </inertial>
+  </link>
+  <joint name="joint_3" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <origin rpy="0 0 0" xyz="0 0 0.6400000000000001"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="200.0" lower="-2.5" upper="2.5" velocity="2.5"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+  <link name="link_3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.24"/>
+      <geometry>
+        <cylinder length="0.48" radius="0.06"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.24"/>
+      <geometry>
+        <cylinder length="0.48" radius="0.06"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.24"/>
+      <mass value="2.5"/>
+      <inertia ixx="0.050249999999999996" ixy="0.0" ixz="0.0" iyy="0.050249999999999996" iyz="0.0" izz="0.0045"/>
+    </inertial>
+  </link>
+  <joint name="joint_4" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <origin rpy="0 0 0" xyz="0 0 0.48"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.0"/>
+    <dynamics damping="0.5" friction="0.5"/>
+  </joint>
+  <link name="link_4">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.16000000000000003"/>
+      <geometry>
+        <cylinder length="0.32000000000000006" radius="0.05"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.16000000000000003"/>
+      <geometry>
+        <cylinder length="0.32000000000000006" radius="0.05"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.16000000000000003"/>
+      <mass value="1.8"/>
+      <inertia ixx="0.016485000000000007" ixy="0.0" ixz="0.0" iyy="0.016485000000000007" iyz="0.0" izz="0.0022500000000000003"/>
+    </inertial>
+  </link>
+  <joint name="joint_5" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <origin rpy="0 0 0" xyz="0 0 0.32000000000000006"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="100.0" lower="-2.0" upper="2.0" velocity="3.0"/>
+    <dynamics damping="0.5" friction="0.5"/>
+  </joint>
+  <link name="link_5">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.08000000000000002"/>
+      <geometry>
+        <cylinder length="0.16000000000000003" radius="0.04"/>
+      </geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.08000000000000002"/>
+      <geometry>
+        <cylinder length="0.16000000000000003" radius="0.04"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.08000000000000002"/>
+      <mass value="1.0"/>
+      <inertia ixx="0.002533333333333334" ixy="0.0" ixz="0.0" iyy="0.002533333333333334" iyz="0.0" izz="0.0008"/>
+    </inertial>
+  </link>
+  <joint name="joint_6" type="revolute">
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <origin rpy="0 0 0" xyz="0 0 0.16000000000000003"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="50.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="4.0"/>
+    <dynamics damping="0.2" friction="0.2"/>
+  </joint>
+  <link name="link_6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.032"/>
+      <geometry>
+        <cylinder length="0.064" radius="0.03"/>
+      </geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.032"/>
+      <geometry>
+        <cylinder length="0.064" radius="0.03"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.032"/>
+      <mass value="0.5"/>
+      <inertia ixx="0.00028316666666666665" ixy="0.0" ixz="0.0" iyy="0.00028316666666666665" iyz="0.0" izz="0.000225"/>
+    </inertial>
+  </link>
+  <link name="tool_link"/>
+  <joint name="tool_joint" type="fixed">
+    <parent link="link_6"/>
+    <child link="tool_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.064"/>
+  </joint>
+  <ros2_control name="GazeboSimSystem" type="system">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    <joint name="joint_1">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint_2">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint_3">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint_4">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint_5">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint_6">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+  <gazebo>
+    <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <parameters>/home/marcin/arm_ws/install/robotarm_description/share/robotarm_description/config/ros2_controllers.yaml</parameters>
+    </plugin>
+  </gazebo>
+</robot>

src/robotarm_description/urdf/robot_arm.urdf.xacro

@@ -3,6 +3,8 @@
 
   <xacro:property name="PI" value="3.1415926535897931"/>
   
+  <xacro:property name="LENGTH_SCALE" value="1.6"/>
+  
   <material name="grey"><color rgba="0.2 0.2 0.2 1.0"/></material>
   <material name="orange"><color rgba="1.0 0.42 0.04 1.0"/></material>
   <material name="blue"><color rgba="0.05 0.35 0.65 1.0"/></material>
@@ -18,13 +20,13 @@
     </inertial>
   </xacro:macro>
 
-  <link name="world"/>
+  <!-- <link name="world"/>
 
   <joint name="world_to_base" type="fixed">
     <parent link="world"/>
     <child link="base_link"/>
     <origin xyz="0 0 0" rpy="0 0 0"/>
-  </joint>
+  </joint> -->
 
   <link name="base_link">
     <visual>
@@ -52,23 +54,23 @@
 
   <link name="link_1">
     <visual>
-      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.08" length="0.3"/></geometry>
+      <geometry><cylinder radius="0.08" length="${0.3 * LENGTH_SCALE}"/></geometry>
       <material name="orange"/>
     </visual>
     <collision>
-      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.08" length="0.3"/></geometry>
+      <geometry><cylinder radius="0.08" length="${0.3 * LENGTH_SCALE}"/></geometry>
     </collision>
-    <xacro:cylinder_inertial mass="4.0" radius="0.08" length="0.3">
+    <xacro:cylinder_inertial mass="4.0" radius="0.08" length="${0.3 * LENGTH_SCALE}">
-      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
     </xacro:cylinder_inertial>
   </link>
 
   <joint name="joint_2" type="revolute">
     <parent link="link_1"/>
     <child link="link_2"/>
-    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <origin xyz="0 0 ${0.3 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 1 0"/>
     <limit lower="-2.0" upper="2.0" effort="300.0" velocity="2.0"/>
     <dynamics damping="1.0" friction="1.0"/>
@@ -76,23 +78,23 @@
 
   <link name="link_2">
     <visual>
-      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.07" length="0.4"/></geometry>
+      <geometry><cylinder radius="0.07" length="${0.4 * LENGTH_SCALE}"/></geometry>
       <material name="blue"/>
     </visual>
     <collision>
-      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.07" length="0.4"/></geometry>
+      <geometry><cylinder radius="0.07" length="${0.4 * LENGTH_SCALE}"/></geometry>
     </collision>
-    <xacro:cylinder_inertial mass="3.5" radius="0.07" length="0.4">
+    <xacro:cylinder_inertial mass="3.5" radius="0.07" length="${0.4 * LENGTH_SCALE}">
-      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
     </xacro:cylinder_inertial>
   </link>
 
   <joint name="joint_3" type="revolute">
     <parent link="link_2"/>
     <child link="link_3"/>
-    <origin xyz="0 0 0.4" rpy="0 0 0"/>
+    <origin xyz="0 0 ${0.4 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 1 0"/>
     <limit lower="-2.5" upper="2.5" effort="200.0" velocity="2.5"/>
     <dynamics damping="1.0" friction="1.0"/>
@@ -100,23 +102,23 @@
 
   <link name="link_3">
     <visual>
-      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.06" length="0.3"/></geometry>
+      <geometry><cylinder radius="0.06" length="${0.3 * LENGTH_SCALE}"/></geometry>
       <material name="orange"/>
     </visual>
     <collision>
-      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.06" length="0.3"/></geometry>
+      <geometry><cylinder radius="0.06" length="${0.3 * LENGTH_SCALE}"/></geometry>
     </collision>
-    <xacro:cylinder_inertial mass="2.5" radius="0.06" length="0.3">
+    <xacro:cylinder_inertial mass="2.5" radius="0.06" length="${0.3 * LENGTH_SCALE}">
-      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
     </xacro:cylinder_inertial>
   </link>
 
   <joint name="joint_4" type="revolute">
     <parent link="link_3"/>
     <child link="link_4"/>
-    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <origin xyz="0 0 ${0.3 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 0 1"/>
     <limit lower="-${PI}" upper="${PI}" effort="100.0" velocity="3.0"/>
     <dynamics damping="0.5" friction="0.5"/>
@@ -124,23 +126,23 @@
 
   <link name="link_4">
     <visual>
-      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.05" length="0.2"/></geometry>
+      <geometry><cylinder radius="0.05" length="${0.2 * LENGTH_SCALE}"/></geometry>
       <material name="blue"/>
     </visual>
     <collision>
-      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.05" length="0.2"/></geometry>
+      <geometry><cylinder radius="0.05" length="${0.2 * LENGTH_SCALE}"/></geometry>
     </collision>
-    <xacro:cylinder_inertial mass="1.8" radius="0.05" length="0.2">
+    <xacro:cylinder_inertial mass="1.8" radius="0.05" length="${0.2 * LENGTH_SCALE}">
-      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
     </xacro:cylinder_inertial>
   </link>
 
   <joint name="joint_5" type="revolute">
     <parent link="link_4"/>
     <child link="link_5"/>
-    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 1 0"/>
     <limit lower="-2.0" upper="2.0" effort="100.0" velocity="3.0"/>
     <dynamics damping="0.5" friction="0.5"/>
@@ -148,23 +150,23 @@
 
   <link name="link_5">
     <visual>
-      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.04" length="0.1"/></geometry>
+      <geometry><cylinder radius="0.04" length="${0.1 * LENGTH_SCALE}"/></geometry>
       <material name="grey"/>
     </visual>
     <collision>
-      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.04" length="0.1"/></geometry>
+      <geometry><cylinder radius="0.04" length="${0.1 * LENGTH_SCALE}"/></geometry>
     </collision>
-    <xacro:cylinder_inertial mass="1.0" radius="0.04" length="0.1">
+    <xacro:cylinder_inertial mass="1.0" radius="0.04" length="${0.1 * LENGTH_SCALE}">
-      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
     </xacro:cylinder_inertial>
   </link>
 
   <joint name="joint_6" type="revolute">
     <parent link="link_5"/>
     <child link="link_6"/>
-    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+    <origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
     <axis xyz="0 0 1"/>
     <limit lower="-${PI}" upper="${PI}" effort="50.0" velocity="4.0"/>
     <dynamics damping="0.2" friction="0.2"/>
@@ -172,16 +174,16 @@
 
   <link name="link_6">
     <visual>
-      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.03" length="0.04"/></geometry>
+      <geometry><cylinder radius="0.03" length="${0.04 * LENGTH_SCALE}"/></geometry>
       <material name="orange"/>
     </visual>
     <collision>
-      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
-      <geometry><cylinder radius="0.03" length="0.04"/></geometry>
+      <geometry><cylinder radius="0.03" length="${0.04 * LENGTH_SCALE}"/></geometry>
     </collision>
-    <xacro:cylinder_inertial mass="0.5" radius="0.03" length="0.04">
+    <xacro:cylinder_inertial mass="0.5" radius="0.03" length="${0.04 * LENGTH_SCALE}">
-      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
     </xacro:cylinder_inertial>
   </link>
 
@@ -189,7 +191,7 @@
   <joint name="tool_joint" type="fixed">
     <parent link="link_6"/>
     <child link="tool_link"/>
-    <origin xyz="0 0 0.04" rpy="0 0 0"/>
+    <origin xyz="0 0 ${0.04 * LENGTH_SCALE}" rpy="0 0 0"/>
   </joint>
 
   <ros2_control name="GazeboSimSystem" type="system">
@@ -247,7 +249,7 @@
   </ros2_control>
 
   <gazebo>
-    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+    <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
       <parameters>$(find robotarm_description)/config/ros2_controllers.yaml</parameters>
     </plugin>
   </gazebo>

src/world_description/corn_field.sdf

@@ -0,0 +1,75 @@
+<sdf version='1.10'>
+  <world name='corn_field_wide'>
+    <physics name='1ms' type='ignored'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <plugin name='gz::sim::systems::Physics' filename='gz-sim-physics-system'/>
+    <plugin name='gz::sim::systems::UserCommands' filename='gz-sim-user-commands-system'/>
+    <plugin name='gz::sim::systems::SceneBroadcaster' filename='gz-sim-scene-broadcaster-system'/>
+    
+    <gravity>0 0 -9.8</gravity>
+    
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>true</shadows>
+      <sky>
+        <clouds>
+          <speed>0.6</speed>
+        </clouds>
+      </sky>
+    </scene>
+
+    <model name='ground_plane'>
+      <static>true</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.2 0.15 0.1 1</ambient>
+            <diffuse>0.25 0.18 0.12 1</diffuse>
+            <specular>0 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <light name='sun' type='directional'>
+      <pose>0 0 10 0 0 0</pose>
+      <cast_shadows>true</cast_shadows>
+      <intensity>1</intensity>
+      <direction>-0.5 0.1 -0.9</direction>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+    </light>
+
+    <include>
+      <uri>file:///home/marcin/arm_ws/src/world_description/harvester.sdf</uri>
+      <pose>-10 -10 3.3 0 0 0</pose>
+    </include>
+
+  </world>
+</sdf>

src/world_description/harvester.sdf

@@ -0,0 +1,156 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='harvester_robot'>
+    <pose>0 0 3.3 0 0 0</pose> 
+    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine> 
+    </plugin>
+
+    <link name='base_link'>
+      <inertial>
+        <mass>150.0</mass> 
+        <inertia>
+          <ixx>128.5</ixx><ixy>0</ixy><ixz>0</ixz>
+          <iyy>200.5</iyy><iyz>0</iyz>
+          <izz>328.0</izz> 
+        </inertia>
+      </inertial>
+      <collision name='collision_left_side'>
+        <pose>0 1.4 0 0 0 0</pose> <geometry><box><size>4.0 0.4 0.2</size></box></geometry> 
+      </collision>
+      <collision name='collision_right_side'>
+        <pose>0 -1.4 0 0 0 0</pose> <geometry><box><size>4.0 0.4 0.2</size></box></geometry> 
+      </collision>
+      <collision name='collision_top_plate'>
+        <pose>0 0 0.5 0 0 0</pose> <geometry><box><size>4.0 2.4 0.1</size></box></geometry> 
+      </collision>
+      <visual name='base_visual'>
+        <geometry><box><size>4.0 3.2 0.2</size></box></geometry> <material><ambient>0.1 0.3 0.6 1</ambient><diffuse>0.1 0.3 0.6 1</diffuse></material> 
+      </visual>
+    </link>
+
+    <link name='axle_fl'><pose>1.8 1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
+    <link name='axle_fr'><pose>1.8 -1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
+    <link name='axle_bl'><pose>-1.8 1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
+    <link name='axle_br'><pose>-1.8 -1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
+
+    <joint name='steer_fl' type='fixed'><parent>base_link</parent><child>axle_fl</child></joint>
+    <joint name='steer_fr' type='fixed'><parent>base_link</parent><child>axle_fr</child></joint>
+    <joint name='steer_bl' type='fixed'><parent>base_link</parent><child>axle_bl</child></joint>
+    <joint name='steer_br' type='fixed'><parent>base_link</parent><child>axle_br</child></joint>
+
+    <link name='wheel_fl'>
+      <pose>1.8 1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
+      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
+        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
+      </collision>
+      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
+    </link>
+
+    <link name='wheel_fr'>
+      <pose>1.8 -1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
+      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
+        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
+      </collision>
+      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
+    </link>
+
+    <link name='wheel_bl'>
+      <pose>-1.8 1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
+      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
+        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
+      </collision>
+      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
+    </link>
+
+    <link name='wheel_br'>
+      <pose>-1.8 -1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
+      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
+        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
+      </collision>
+      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
+    </link>
+
+    <link name="lidar_left_link">
+      <pose>-1.0 0.5 -2.4 0 0 -0.785398</pose>
+      <inertial><mass>0.1</mass><inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia></inertial>
+      <visual name="visual"><geometry><box><size>0.1 0.1 0.1</size></box></geometry><material><ambient>0 1 0 1</ambient></material></visual>
+      <sensor name='gpu_lidar_l' type='gpu_lidar'>
+        <pose>0 0 0 0 0 0</pose>
+        <frame_id>lidar_left_link</frame_id>
+        <topic>lidar_left</topic>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal><samples>180</samples><resolution>1</resolution><min_angle>-0.785398</min_angle><max_angle>0.785398</max_angle></horizontal>
+          </scan>
+          <range><min>0.08</min><max>10</max><resolution>0.01</resolution></range>
+        </lidar>
+      </sensor>
+    </link>
+
+    <link name="lidar_right_link">
+      <pose>-1.0 -0.5 -2.4 0 0 0.785398</pose>
+      <inertial><mass>0.1</mass><inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia></inertial>
+      <visual name="visual"><geometry><box><size>0.1 0.1 0.1</size></box></geometry><material><ambient>1 0.5 0 1</ambient></material></visual>
+      <sensor name='gpu_lidar_r' type='gpu_lidar'>
+        <pose>0 0 0 0 0 0</pose>
+        <frame_id>lidar_right_link</frame_id>
+        <topic>lidar_right</topic>
+        <update_rate>10</update_rate>
+        <lidar>
+          <scan>
+            <horizontal><samples>180</samples><resolution>1</resolution><min_angle>-0.785398</min_angle><max_angle>0.785398</max_angle></horizontal>
+          </scan>
+          <range><min>0.08</min><max>10</max><resolution>0.01</resolution></range>
+        </lidar>
+      </sensor>
+    </link>
+
+    <link name="camera_link">
+      <pose>1.95 0 -1 0 0.15 0</pose> <inertial><mass>0.1</mass> <inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia> </inertial>
+      <visual name="visual"><geometry><box><size>0.1 0.1 0.1</size></box></geometry> <material><ambient>0.1 0.1 0.1 1</ambient></material> </visual>
+      <sensor name="camera" type="camera">
+        <camera><horizontal_fov>1.047</horizontal_fov> <image><width>1920</width><height>1080</height></image> <clip><near>0.1</near><far>100</far></clip> </camera>
+        <always_on>1</always_on> <update_rate>30</update_rate> <visualize>true</visualize> <topic>camera</topic> 
+      </sensor>
+    </link>
+
+    <link name="gps_link">
+      <pose>0 0 0.2 0 0 0</pose> <inertial><mass>0.1</mass> <inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia> </inertial>
+      <visual name="visual"><geometry><cylinder><radius>0.1</radius><length>0.05</length></cylinder></geometry> <material><ambient>0.8 0.1 0.1 1</ambient></material> </visual>
+      <sensor name="navsat" type="navsat"><always_on>1</always_on> <update_rate>10</update_rate> <topic>gps</topic> </sensor>
+    </link>
+
+    <joint name="lidar_left_joint" type="fixed"><parent>base_link</parent><child>lidar_left_link</child></joint>
+    <joint name="lidar_right_joint" type="fixed"><parent>base_link</parent><child>lidar_right_link</child></joint>
+    <joint name="gps_joint" type="fixed"><parent>base_link</parent><child>gps_link</child></joint> 
+    <joint name="camera_joint" type="fixed"><parent>base_link</parent><child>camera_link</child></joint> 
+
+    <joint name='wheel_fl_joint' type='revolute'><parent>axle_fl</parent><child>wheel_fl</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
+    <joint name='wheel_fr_joint' type='revolute'><parent>axle_fr</parent><child>wheel_fr</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
+    <joint name='wheel_bl_joint' type='revolute'><parent>axle_bl</parent><child>wheel_bl</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
+    <joint name='wheel_br_joint' type='revolute'><parent>axle_br</parent><child>wheel_br</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
+  
+    <include>
+      <uri>file:///home/marcin/arm_ws/src/robotarm_description/urdf/robot_arm.urdf</uri>
+      <name>arm</name>
+      <pose>0 0 -0.1 3.14159 0 0</pose> 
+    </include>
+
+    <joint name="chassis_to_arm_joint" type="fixed">
+      <!-- <pose relative_to="base_link">0 0 -0.2 0 0 0</pose> -->
+      <parent>base_link</parent>
+      <child>arm::base_link</child>
+    </joint>
+
+    <plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
+      <left_joint>wheel_fl_joint</left_joint><left_joint>wheel_bl_joint</left_joint>
+      <right_joint>wheel_fr_joint</right_joint><right_joint>wheel_br_joint</right_joint>
+      <wheel_separation>2.8</wheel_separation> <wheel_radius>0.4</wheel_radius> <max_wheel_torque>500.0</max_wheel_torque>
+      <max_linear_acceleration>5.0</max_linear_acceleration><max_angular_acceleration>5.0</max_angular_acceleration>
+      <topic>/cmd_vel</topic> 
+    </plugin>
+
+  </model>
+</sdf>

src/world_description/mission.py

@@ -0,0 +1,361 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+import math
+import time
+import sys
+import numpy as np
+
+try:
+    import pygame
+except ImportError:
+    print("Błąd: Brak biblioteki pygame. Zainstaluj wpisując: pip3 install pygame")
+    sys.exit(1)
+
+class HarvesterStateMachine(Node):
+    def __init__(self):
+        super().__init__('harvester_sm_node')
+
+        # --- KONFIGURACJA TEMATÓW ---
+        odom_topic_name = '/odom' 
+        cmd_vel_topic_name = '/cmd_vel'
+        
+        qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10
+        )
+
+        # --- PUBLISHERS & SUBSCRIBERS ---
+        self.cmd_vel_pub = self.create_publisher(Twist, cmd_vel_topic_name, 10)
+        self.odom_sub = self.create_subscription(Odometry, odom_topic_name, self.odom_callback, qos_profile)
+        
+        # Subskrypcja dwóch krzyżujących się LiDARów zgodnie z Twoim planem
+        self.lidar_left_sub = self.create_subscription(LaserScan, '/lidar_left', self.lidar_left_callback, qos_profile)
+        self.lidar_right_sub = self.create_subscription(LaserScan, '/lidar_right', self.lidar_right_callback, qos_profile)
+
+        # --- MASZYNA STANÓW ---
+        self.STATE_INITIALIZING = "INITIALIZING"
+        self.STATE_GPS_DRIVE    = "GPS_DRIVE"
+        self.STATE_LIDAR_DRIVE  = "LIDAR_DRIVE"
+        self.STATE_HARVESTING   = "HARVESTING"
+        self.STATE_MISSION_DONE = "MISSION_DONE"
+        self.current_state = self.STATE_INITIALIZING
+
+        # --- ZMIENNE ODOMETRII ---
+        self.robot_x, self.robot_y, self.robot_yaw = 0.0, 0.0, 0.0
+        self.has_odom = False
+        self.start_offset_x, self.start_offset_y = 0.0, 0.0
+
+        # --- CHMURY PUNKTÓW PO FUZJI (W UKŁADZIE ROBOTA) ---
+        self.pts_L = ([], [])   # Ściana lewa (zlewana z obu laserów)
+        self.pts_R = ([], [])   # Ściana prawa (zlewana z obu laserów)
+        
+        self.avg_left = 0.0
+        self.avg_right = 0.0
+        self.end_of_row_detected = False
+
+        # Bufory na surowe wiadomości ROS2
+        self.latest_left_scan = None
+        self.latest_right_scan = None
+
+        # --- LOGIKA PRZEJAZDU ---
+        self.lidar_start_x, self.lidar_start_y = 0.0, 0.0
+        self.lidar_enter_time = 0.0
+        self.harvesting_done_in_this_row = False
+        self.harvest_trigger_distance = 6.0  
+
+        # --- PUNKTY DOCELOWE ---
+        self.global_waypoints = [{"x": -3.0, "y": 0.8}, {"x": -3.0, "y": 2.4}]
+        self.local_waypoints = []
+        self.current_wp_idx = 0
+        self.position_reached = False 
+
+        # --- PARAMETRY REGULACJI ---
+        self.target_pos_tolerance = 0.4      
+        self.target_heading_tolerance = 0.05  
+        self.lidar_speed = 0.25               
+        self.lidar_kp = 0.35                  
+        self.deadzone = 0.08                  
+        
+        self.control_timer = self.create_timer(0.1, self.control_loop)
+
+        # --- PYGAME WIZUALIZACJA ---
+        pygame.init()
+        self.win_size = 600
+        self.screen = pygame.display.set_mode((self.win_size, self.win_size))
+        pygame.display.set_caption("Kombajn: Lokalna Fuzja Układu Współrzędnych (Cross-Eye)")
+
+    def odom_callback(self, msg: Odometry):
+        self.robot_x = msg.pose.pose.position.x
+        self.robot_y = msg.pose.pose.position.y
+        
+        q = msg.pose.pose.orientation
+        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
+        self.robot_yaw = math.atan2(siny_cosp, cosy_cosp)
+        
+        if not self.has_odom:
+            self.start_offset_x, self.start_offset_y = self.robot_x, self.robot_y
+            gazebo_spawn_x, gazebo_spawn_y = -10.0, -10.0
+
+            for wp in self.global_waypoints:
+                self.local_waypoints.append({
+                    "x": self.start_offset_x + (wp["x"] - gazebo_spawn_x),
+                    "y": self.start_offset_y + (wp["y"] - gazebo_spawn_y)
+                })
+            self.has_odom = True
+
+    def lidar_left_callback(self, msg: LaserScan):
+        self.latest_left_scan = msg
+
+    def lidar_right_callback(self, msg: LaserScan):
+        self.latest_right_scan = msg
+
+    def process_lidar_fusion(self):
+        if not self.latest_left_scan or not self.latest_right_scan:
+            return
+
+        x_left_wall, y_left_wall = [], []
+        x_right_wall, y_right_wall = [], []
+
+        # --- SENSOR 1: LEWY LIDAR (Nowe pozycje!) ---
+        l_msg = self.latest_left_scan
+        l_x, l_y, l_yaw = -1.0, 0.5, -0.785398  # <--- TUTAJ ZMIANA
+        
+        # ... (reszta pętli dla lewego lasera bez zmian) ...
+        
+        for i, distance in enumerate(l_msg.ranges):
+            if math.isinf(distance) or math.isnan(distance) or distance < 3.0 or distance > l_msg.range_max: 
+                continue
+            angle = l_msg.angle_min + i * l_msg.angle_increment
+            
+            # Pozycja punktu względem samego sensora
+            xs = distance * math.cos(angle)
+            ys = distance * math.sin(angle)
+            
+            # Rzutowanie (Transformacja macierzowa) na układ robota (środek ramy)
+            xr = l_x + (xs * math.cos(l_yaw) - ys * math.sin(l_yaw))
+            yr = l_y + (xs * math.sin(l_yaw) + ys * math.cos(l_yaw))
+            
+            # Segregacja: jeśli punkt wylądował po lewej stronie robota (Yr > 0), to lewa ściana, inaczej prawa
+            if yr > 0.0:
+                x_left_wall.append(xr)
+                y_left_wall.append(yr)
+            else:
+                x_right_wall.append(xr)
+                y_right_wall.append(yr)
+
+        r_msg = self.latest_right_scan
+        r_x, r_y, r_yaw = -1.0, -0.5, 0.785398  # <--- TUTAJ ZMIANA
+        
+        # ... (reszta pętli dla prawego lasera bez zmian) ...
+        
+        for i, distance in enumerate(r_msg.ranges):
+            if math.isinf(distance) or math.isnan(distance) or distance < 3.0 or distance > r_msg.range_max: 
+                continue
+            angle = r_msg.angle_min + i * r_msg.angle_increment
+            
+            xs = distance * math.cos(angle)
+            ys = distance * math.sin(angle)
+            
+            # Rzutowanie na układ robota
+            xr = r_x + (xs * math.cos(r_yaw) - ys * math.sin(r_yaw))
+            yr = r_y + (xs * math.sin(r_yaw) + ys * math.cos(r_yaw))
+            
+            if yr > 0.0:
+                x_left_wall.append(xr)
+                y_left_wall.append(yr)
+            else:
+                x_right_wall.append(xr)
+                y_right_wall.append(yr)
+
+        self.pts_L = (x_left_wall, y_left_wall)
+        self.pts_R = (x_right_wall, y_right_wall)
+
+        # Wyliczanie średnich odległości bocznych korytarza dla regulatora PID
+        min_points = 4
+        if len(y_left_wall) < min_points and len(y_right_wall) < min_points:
+            self.end_of_row_detected = True
+            self.avg_left, self.avg_right = 1.4, 1.4
+        else:
+            self.end_of_row_detected = False
+            # Średnia odległość punktów od osi podłużnej robota (Y=0)
+            self.avg_left = sum(y_left_wall) / len(y_left_wall) if y_left_wall else 1.4
+            self.avg_right = abs(sum(y_right_wall) / len(y_right_wall)) if y_right_wall else 1.4
+
+    def control_loop(self):
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                pygame.quit()
+                sys.exit()
+
+        # Dynamiczny start w oparciu o zebrany komplet tematów z mostka
+        if self.current_state == self.STATE_INITIALIZING:
+            if self.has_odom and self.latest_left_scan is not None and self.latest_right_scan is not None:
+                self.current_state = self.STATE_GPS_DRIVE
+            self.draw_pygame_window()
+            return
+
+        # Wykonaj fuzję współrzędnych lokalnych przed podjęciem decyzji o ruchu
+        self.process_lidar_fusion()
+
+        if self.current_state == self.STATE_GPS_DRIVE: self.execute_gps_drive()
+        elif self.current_state == self.STATE_LIDAR_DRIVE: self.execute_lidar_drive()
+        elif self.current_state == self.STATE_HARVESTING: self.execute_harvesting()
+        elif self.current_state == self.STATE_MISSION_DONE: self.stop_robot()
+
+        self.draw_pygame_window()
+
+    def execute_gps_drive(self):
+        if self.current_wp_idx >= len(self.local_waypoints):
+            self.current_state = self.STATE_MISSION_DONE
+            return
+
+        target = self.local_waypoints[self.current_wp_idx]
+        cmd = Twist()
+
+        if not self.position_reached:
+            dx, dy = target["x"] - self.robot_x, target["y"] - self.robot_y
+            distance = math.sqrt(dx**2 + dy**2)
+            desired_yaw = math.atan2(dy, dx)
+            yaw_error = math.atan2(math.sin(desired_yaw - self.robot_yaw), math.cos(desired_yaw - self.robot_yaw))
+
+            if distance < self.target_pos_tolerance:
+                self.stop_robot()
+                self.position_reached = True
+                return
+
+            if abs(yaw_error) > 0.15:
+                cmd.angular.z = max(min(0.6 * yaw_error, 0.5), -0.5)
+            else:
+                cmd.linear.x = 0.4  
+                cmd.angular.z = 0.8 * yaw_error  
+            self.cmd_vel_pub.publish(cmd)
+        else:
+            yaw_error = math.atan2(math.sin(0.0 - self.robot_yaw), math.cos(0.0 - self.robot_yaw))
+            if abs(yaw_error) < self.target_heading_tolerance:
+                self.stop_robot()
+                self.position_reached = False
+                self.lidar_start_x, self.lidar_start_y = self.robot_x, self.robot_y
+                self.lidar_enter_time = time.time()
+                self.harvesting_done_in_this_row = False
+                self.current_state = self.STATE_LIDAR_DRIVE
+                return
+
+            cmd.angular.z = max(min(0.5 * yaw_error, 0.4), -0.4)
+            if 0 < cmd.angular.z < 0.15: cmd.angular.z = 0.15
+            if -0.15 < cmd.angular.z < 0: cmd.angular.z = -0.15
+            self.cmd_vel_pub.publish(cmd)
+
+    def execute_lidar_drive(self):
+        cmd = Twist()
+        time_in_lidar = time.time() - self.lidar_enter_time
+        
+        if self.end_of_row_detected and time_in_lidar > 6.0:
+            self.stop_robot()
+            self.get_logger().info("Wyjazd z korytarza. Powrót do nawigacji GPS.")
+            self.current_wp_idx += 1  
+            self.current_state = self.STATE_GPS_DRIVE
+            return
+
+        dist_traveled = math.sqrt((self.robot_x - self.lidar_start_x)**2 + (self.robot_y - self.lidar_start_y)**2)
+        if dist_traveled >= self.harvest_trigger_distance and not self.harvesting_done_in_this_row:
+            self.stop_robot()
+            self.current_state = self.STATE_HARVESTING
+            return
+
+        # Logika centrowania oparta o zunifikowane odległości boczne Y po fuzji
+        diff = self.avg_left - self.avg_right
+        cmd.linear.x = self.lidar_speed
+
+        if abs(diff) < self.deadzone:
+            cmd.angular.z = 0.0  
+        else:
+            cmd.angular.z = self.lidar_kp * diff  
+
+        cmd.angular.z = max(min(cmd.angular.z, 0.2), -0.2)
+        self.cmd_vel_pub.publish(cmd)
+
+    def execute_harvesting(self):
+        self.stop_robot()
+        time.sleep(2.0)
+        self.harvesting_done_in_this_row = True
+        self.current_state = self.STATE_LIDAR_DRIVE
+
+    def stop_robot(self):
+        cmd = Twist()
+        cmd.linear.x, cmd.angular.z = 0.0, 0.0
+        self.cmd_vel_pub.publish(cmd)
+
+    def draw_pygame_window(self):
+        """Wizualizacja zrzutowanych punktów w lokalnej macierzy robota"""
+        self.screen.fill((12, 16, 24))
+        
+        # Środek okna reprezentuje geometryczny środek kombajnu
+        cx, cy = self.win_size // 2, self.win_size // 2 + 50 
+        scale = 55.0  
+
+        # Siatka radarowa (okręgi co 1 metr)
+        for r in range(1, 8):
+            pygame.draw.circle(self.screen, (25, 35, 45), (cx, cy), int(r * scale), 1)
+
+        # Rysowanie obrysu kombajnu (SDF: długość 4.0, szerokość 3.2)
+        robot_w = int(3.2 * scale)
+        robot_h = int(4.0 * scale)
+        rx = cx - robot_w // 2
+        ry = cy - robot_h // 2
+        pygame.draw.rect(self.screen, (55, 65, 80), pygame.Rect(rx, ry, robot_w, robot_h), 2)
+        
+        # Pozycja montażowa fizyczna dwóch wewnętrznych LiDARów (X=-2.0, Y= +/- 1.5)
+        pygame.draw.circle(self.screen, (0, 255, 0), (cx - int(1.5*scale), cy + int(2.0*scale)), 5)   # Lewy
+        pygame.draw.circle(self.screen, (255, 150, 0), (cx - int(-1.5*scale), cy + int(2.0*scale)), 5) # Prawy
+
+        font = pygame.font.SysFont('Monospace', 13, bold=True)
+        if self.current_state == self.STATE_INITIALIZING:
+            status_text = "INITIALIZING: Oczekiwanie na"
+            if not self.has_odom: status_text += " [ODOM]"
+            if self.latest_left_scan is None: status_text += " [LIDAR_L]"
+            if self.latest_right_scan is None: status_text += " [LIDAR_R]"
+            self.screen.blit(font.render(status_text, True, (255, 140, 0)), (15, 15))
+            pygame.display.flip()
+            return
+
+        # Rysowanie przeliczonych chmur punktów (Lewa ściana = zielona, Prawa = pomarańczowa)
+        for x, y in zip(self.pts_L[0], self.pts_L[1]):
+            px = cx - int(y * scale)
+            py = cy - int(x * scale)
+            if 0 <= px < self.win_size and 0 <= py < self.win_size:
+                pygame.draw.circle(self.screen, (100, 255, 100), (px, py), 2)
+
+        for x, y in zip(self.pts_R[0], self.pts_R[1]):
+            px = cx - int(y * scale)
+            py = cy - int(x * scale)
+            if 0 <= px < self.win_size and 0 <= py < self.win_size:
+                pygame.draw.circle(self.screen, (255, 165, 0), (px, py), 2)
+
+        # Wyświetlanie tekstów diagnostycznych
+        self.screen.blit(font.render(f"STAN: {self.current_state}", True, (255, 255, 255)), (15, 15))
+        self.screen.blit(font.render(f"DYSTANS OD LEWEJ ŚCIANY:  {self.avg_left:.2f}m", True, (100, 255, 100)), (15, 35))
+        self.screen.blit(font.render(f"DYSTANS OD PRAWEJ ŚCIANY: {self.avg_right:.2f}m", True, (255, 165, 0)), (15, 55))
+        pygame.display.flip()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = HarvesterStateMachine()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.stop_robot()
+        node.destroy_node()
+        rclpy.shutdown()
+        pygame.quit()
+
+if __name__ == '__main__':
+    main()

src/world_description/models/Mud Box/materials/scripts/mud.material

@@ -0,0 +1,19 @@
+material vrc/mud
+{
+	technique
+	{
+		pass
+		{
+			ambient 0.5 0.5 0.5 1.0
+			diffuse 0.5 0.5 0.5 1.0
+			specular 0.2 0.2 0.2 1.0 12.5
+
+			texture_unit
+			{
+				texture mud_soft_leaves.png
+				filtering anistropic
+				max_anisotropy 16
+			}
+		}
+	}
+}

src/world_description/models/Mud Box/model-1_4.sdf

@@ -0,0 +1,80 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+  <model name="mud_box">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>8 10 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual_1">
+        <pose>-2 2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="visual_2">
+        <pose>2 2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="visual_3">
+        <pose>2 -2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="visual_4">
+        <pose>-2 -2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+
+    </link>
+  </model>
+</sdf>

src/world_description/models/Mud Box/model.config

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Mud Box</name>
+  <version>1.0</version>
+  <sdf version="1.4">model-1_4.sdf</sdf>
+  <sdf version="1.5">model.sdf</sdf>
+
+  <author>
+    <name>Thomas Koletschka</name>
+    <email>thomas.koletschka@gmail.com</email>
+  </author>
+
+  <description>
+    A mud textured plane.
+  </description>
+
+</model>

src/world_description/models/Mud Box/model.sdf

@@ -0,0 +1,79 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="mud_box">
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>8 10 0.2</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual_1">
+        <pose>-2 2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="visual_2">
+        <pose>2 2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="visual_3">
+        <pose>2 -2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="visual_4">
+        <pose>-2 -2.5 0 0 0 0</pose>
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <box>
+            <size>4 5 0.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://mud_box/materials/scripts</uri>
+            <uri>model://mud_box/materials/textures</uri>
+            <name>vrc/mud</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

src/world_description/models/corn.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+<model name="Corn">
+  <static>true</static>
+  <link name="link">
+    <visual name="visual">
+      <geometry>
+        <mesh>
+          <uri>meshes/scene.gltf</uri>
+        </mesh>
+      </geometry>
+    </visual>
+    </link>
+
+    <collision name="lidar_blocker">
+      <pose>0 0 0.5 0 0 0</pose> <geometry>
+        <cylinder>
+          <radius>0.15</radius> <length>1.0</length>
+        </cylinder>
+      </geometry>
+    </collision>
+
+</model>
+</sdf>

src/world_description/models/meshes/license.txt

@@ -0,0 +1,11 @@
+Model Information:
+* title:	Corn! Corn! Corn!
+* source:	https://sketchfab.com/3d-models/corn-corn-corn-10187bc37c9e42ef8770b28452ee7cd3
+* author:	Tiia Tuulia (https://sketchfab.com/tiiatuulia)
+
+Model License:
+* license type:	CC-BY-4.0 (http://creativecommons.org/licenses/by/4.0/)
+* requirements:	Author must be credited. Commercial use is allowed.
+
+If you use this 3D model in your project be sure to copy paste this credit wherever you share it:
+This work is based on "Corn! Corn! Corn!" (https://sketchfab.com/3d-models/corn-corn-corn-10187bc37c9e42ef8770b28452ee7cd3) by Tiia Tuulia (https://sketchfab.com/tiiatuulia) licensed under CC-BY-4.0 (http://creativecommons.org/licenses/by/4.0/)

src/world_description/models/meshes/scene.gltf

@@ -0,0 +1,394 @@
+{
+  "accessors": [
+    {
+      "bufferView": 2,
+      "componentType": 5126,
+      "count": 2029,
+      "max": [
+        10.655570983886719,
+        11.136487007141113,
+        4.473674774169922
+      ],
+      "min": [
+        -11.730097770690918,
+        -8.04345703125,
+        -41.532630920410156
+      ],
+      "type": "VEC3"
+    },
+    {
+      "bufferView": 2,
+      "byteOffset": 24348,
+      "componentType": 5126,
+      "count": 2029,
+      "max": [
+        0.9976066946983337,
+        1.0,
+        0.9962291717529297
+      ],
+      "min": [
+        -0.9968279004096985,
+        -0.994667649269104,
+        -0.990868330001831
+      ],
+      "type": "VEC3"
+    },
+    {
+      "bufferView": 1,
+      "componentType": 5126,
+      "count": 2029,
+      "max": [
+        0.5003870129585266,
+        0.9281949996948242
+      ],
+      "min": [
+        0.062497999519109726,
+        0.058719001710414886
+      ],
+      "type": "VEC2"
+    },
+    {
+      "bufferView": 0,
+      "componentType": 5125,
+      "count": 7188,
+      "type": "SCALAR"
+    },
+    {
+      "bufferView": 2,
+      "byteOffset": 48696,
+      "componentType": 5126,
+      "count": 146,
+      "max": [
+        3.683738946914673,
+        0.2560949921607971,
+        -1.8365540504455566
+      ],
+      "min": [
+        -1.6435259580612183,
+        -11.871248245239258,
+        -31.080829620361328
+      ],
+      "type": "VEC3"
+    },
+    {
+      "bufferView": 2,
+      "byteOffset": 50448,
+      "componentType": 5126,
+      "count": 146,
+      "max": [
+        0.9527933597564697,
+        0.8330304622650146,
+        0.9927104711532593
+      ],
+      "min": [
+        -0.9898576140403748,
+        -0.9799456596374512,
+        -0.9875993728637695
+      ],
+      "type": "VEC3"
+    },
+    {
+      "bufferView": 1,
+      "byteOffset": 16232,
+      "componentType": 5126,
+      "count": 146,
+      "max": [
+        0.1911740005016327,
+        0.9113199710845947
+      ],
+      "min": [
+        0.062497999519109726,
+        0.058719001710414886
+      ],
+      "type": "VEC2"
+    },
+    {
+      "bufferView": 0,
+      "byteOffset": 28752,
+      "componentType": 5125,
+      "count": 522,
+      "type": "SCALAR"
+    },
+    {
+      "bufferView": 3,
+      "componentType": 5126,
+      "count": 1,
+      "max": [
+        0.0416666679084301
+      ],
+      "min": [
+        0.0416666679084301
+      ],
+      "type": "SCALAR"
+    },
+    {
+      "bufferView": 4,
+      "componentType": 5126,
+      "count": 1,
+      "max": [
+        -0.27267909049987793,
+        0.6607204675674438,
+        0.6418024897575378,
+        0.2778203785419464
+      ],
+      "min": [
+        -0.27267909049987793,
+        0.6607204675674438,
+        0.6418024897575378,
+        0.2778203785419464
+      ],
+      "type": "VEC4"
+    }
+  ],
+  "animations": [
+    {
+      "channels": [
+        {
+          "sampler": 0,
+          "target": {
+            "node": 4,
+            "path": "rotation"
+          }
+        }
+      ],
+      "name": "Default Take",
+      "samplers": [
+        {
+          "input": 8,
+          "interpolation": "LINEAR",
+          "output": 9
+        }
+      ]
+    }
+  ],
+  "asset": {
+    "extras": {
+      "author": "Tiia Tuulia (https://sketchfab.com/tiiatuulia)",
+      "license": "CC-BY-4.0 (http://creativecommons.org/licenses/by/4.0/)",
+      "source": "https://sketchfab.com/3d-models/corn-corn-corn-10187bc37c9e42ef8770b28452ee7cd3",
+      "title": "Corn! Corn! Corn!"
+    },
+    "generator": "Sketchfab-12.68.0",
+    "version": "2.0"
+  },
+  "bufferViews": [
+    {
+      "buffer": 0,
+      "byteLength": 30840,
+      "name": "floatBufferViews",
+      "target": 34963
+    },
+    {
+      "buffer": 0,
+      "byteLength": 17400,
+      "byteOffset": 30840,
+      "byteStride": 8,
+      "name": "floatBufferViews",
+      "target": 34962
+    },
+    {
+      "buffer": 0,
+      "byteLength": 52200,
+      "byteOffset": 48240,
+      "byteStride": 12,
+      "name": "floatBufferViews",
+      "target": 34962
+    },
+    {
+      "buffer": 0,
+      "byteLength": 4,
+      "byteOffset": 100440,
+      "name": "floatBufferViews"
+    },
+    {
+      "buffer": 0,
+      "byteLength": 16,
+      "byteOffset": 100444,
+      "byteStride": 16,
+      "name": "floatBufferViews"
+    }
+  ],
+  "buffers": [
+    {
+      "byteLength": 100460,
+      "uri": "scene.bin"
+    }
+  ],
+  "images": [
+    {
+      "uri": "textures/corn_plant__corn_texture_png_baseColor.png"
+    }
+  ],
+  "materials": [
+    {
+      "doubleSided": true,
+      "name": "corn_plant__corn_texture_png",
+      "pbrMetallicRoughness": {
+        "baseColorTexture": {
+          "index": 0
+        },
+        "metallicFactor": 0.0,
+        "roughnessFactor": 0.6541539634146342
+      }
+    },
+    {
+      "doubleSided": true,
+      "name": "corn_plant_001__corn_texture_png_001",
+      "pbrMetallicRoughness": {
+        "baseColorTexture": {
+          "index": 0
+        },
+        "metallicFactor": 0.0,
+        "roughnessFactor": 0.6
+      }
+    }
+  ],
+  "meshes": [
+    {
+      "name": "Circle_020_corn_plant__corn_texture_png_0",
+      "primitives": [
+        {
+          "attributes": {
+            "NORMAL": 1,
+            "POSITION": 0,
+            "TEXCOORD_0": 2
+          },
+          "indices": 3,
+          "material": 0,
+          "mode": 4
+        }
+      ]
+    },
+    {
+      "name": "Circle_020_corn_plant_001__corn_texture_png_001_0",
+      "primitives": [
+        {
+          "attributes": {
+            "NORMAL": 5,
+            "POSITION": 4,
+            "TEXCOORD_0": 6
+          },
+          "indices": 7,
+          "material": 1,
+          "mode": 4
+        }
+      ]
+    }
+  ],
+  "nodes": [
+    {
+      "children": [
+        1
+      ],
+      "matrix": [
+        0.6242683529853821,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        1.3861541980583915e-16,
+        -0.6242683529853821,
+        0.0,
+        0.0,
+        0.6242683529853821,
+        1.3861541980583915e-16,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        1.0
+      ],
+      "name": "Sketchfab_model"
+    },
+    {
+      "children": [
+        2
+      ],
+      "matrix": [
+        1.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        1.0,
+        0.0,
+        0.0,
+        -1.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        1.0
+      ],
+      "name": "fdd38405c86b44e49e5f7a62b0d2f0f7.fbx"
+    },
+    {
+      "children": [
+        3
+      ],
+      "name": "Object_2"
+    },
+    {
+      "children": [
+        4
+      ],
+      "name": "RootNode"
+    },
+    {
+      "children": [
+        5,
+        6
+      ],
+      "name": "Circle_020",
+      "rotation": [
+        -0.27267906069755554,
+        0.6607205271720886,
+        0.6418024301528931,
+        0.277820348739624
+      ],
+      "scale": [
+        0.08378752321004868,
+        0.08378753066062927,
+        0.08378753066062927
+      ],
+      "translation": [
+        0.03753768280148506,
+        3.4483883380889893,
+        0.0886821448802948
+      ]
+    },
+    {
+      "mesh": 0,
+      "name": "Circle_020_corn_plant__corn_texture_png_0"
+    },
+    {
+      "mesh": 1,
+      "name": "Circle_020_corn_plant_001__corn_texture_png_001_0"
+    }
+  ],
+  "samplers": [
+    {
+      "magFilter": 9729,
+      "minFilter": 9987,
+      "wrapS": 10497,
+      "wrapT": 10497
+    }
+  ],
+  "scene": 0,
+  "scenes": [
+    {
+      "name": "Sketchfab_Scene",
+      "nodes": [
+        0
+      ]
+    }
+  ],
+  "textures": [
+    {
+      "sampler": 0,
+      "source": 0
+    }
+  ]
+}

src/world_description/models/model.config

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Corn</name>
+  <version>1.0</version>
+  <sdf version="1.6">corn.sdf</sdf>
+
+  <description>
+    Corn plant
+  </description>
+
+</model>