reference frame

src/arm_harverster/CMakeLists.txt

@@ -7,6 +7,7 @@ find_package(ament_cmake REQUIRED)
 install(PROGRAMS
   scripts/move_to_point.py
   scripts/arm_point_controller.py
+  scripts/mission.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/arm_harverster/scripts/arm_point_controller.py

@@ -70,7 +70,7 @@ class CameraArmController(Node):
             rclpy.spin_once(self, timeout_sec=0.1)
 
         req = GetPositionFK.Request()
-        req.header.frame_id = "world"
+        req.header.frame_id = "base_link"
         req.fk_link_names = ["tool_link"] # Name of the tip frame to track
         
         # Pack current joint configuration into the request
@@ -100,14 +100,14 @@ class CameraArmController(Node):
         goal_msg.request.pipeline_id = "ompl"
         
         target_pose = PoseStamped()
-        target_pose.header.frame_id = "world"
+        target_pose.header.frame_id = "base_link"
         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.header.frame_id = "base_link"
         position_constraint.link_name = "tool_link"
         
         box = SolidPrimitive()
@@ -126,7 +126,7 @@ class CameraArmController(Node):
         position_constraint.weight = 1.0
 
         orient_constraint = OrientationConstraint()
-        orient_constraint.header.frame_id = "world"
+        orient_constraint.header.frame_id = "base_link"
         orient_constraint.link_name = "tool_link"
         orient_constraint.orientation = target_pose.pose.orientation
         
@@ -145,7 +145,7 @@ class CameraArmController(Node):
         
         goal_handle = send_goal_future.result()
         if not goal_handle.accepted:
-            self.get_logger().error("❌ Point-to-Point goal rejected by MoveIt!")
+            self.get_logger().error("Point-to-Point goal rejected by MoveIt!")
             return False
 
         self.get_logger().info("Goal accepted! Executing path...")
@@ -168,12 +168,12 @@ class CameraArmController(Node):
         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"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.header.frame_id = "base_link"
         req.start_state.is_diff = True 
         
         # Target waypoint calculated using live variables
@@ -193,7 +193,7 @@ class CameraArmController(Node):
         
         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.")
+            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
 
@@ -208,10 +208,10 @@ class CameraArmController(Node):
         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!")
+            self.get_logger().info(" Straight camera sweep complete!")
             return True
         else:
-            self.get_logger().error("❌ Trajectory execution rejected.")
+            self.get_logger().error("Trajectory execution rejected.")
             return False
 
 def main(args=None):
@@ -219,14 +219,14 @@ def main(args=None):
     node = CameraArmController()
 
     # 1. Send to starting exploration zone
-    success = node.send_pt_to_pt_goal(0.6, 0.0, 0.5)
+    success = node.send_pt_to_pt_goal(-0.4, -0.3, 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.execute_straight_camera_sweep(distance=0.2)
 
     node.destroy_node()
     rclpy.shutdown()

src/arm_harverster/scripts/mission.py

@@ -20,7 +20,6 @@ class HarvesterStateMachine(Node):
     def __init__(self):
         super().__init__('harvester_sm_node')
 
-        # --- KONFIGURACJA TEMATÓW ---
         odom_topic_name = '/odom' 
         cmd_vel_topic_name = '/cmd_vel'
         
@@ -30,15 +29,11 @@ class HarvesterStateMachine(Node):
             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"
@@ -46,36 +41,32 @@ class HarvesterStateMachine(Node):
         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 = ([], [])
-        self.pts_L = ([], [])   # Ściana lewa (zlewana z obu laserów)
+        self.pts_R = ([], [])
-        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 ---
+        #   PUNKTY DOCELOWE
-        self.global_waypoints = [{"x": -3.0, "y": 0.8}, {"x": -3.0, "y": 2.4}]
+        self.global_waypoints = [{"x": -3.0, "y": 0.8}, {"x": -5.0, "y": -5}]
         self.local_waypoints = []
         self.current_wp_idx = 0
         self.position_reached = False 
 
-        # --- PARAMETRY REGULACJI ---
+        #   PARAMETRY REGULACJI
         self.target_pos_tolerance = 0.4      
         self.target_heading_tolerance = 0.05  
         self.lidar_speed = 0.25               
@@ -84,11 +75,11 @@ class HarvesterStateMachine(Node):
         
         self.control_timer = self.create_timer(0.1, self.control_loop)
 
-        # --- PYGAME WIZUALIZACJA ---
+        #   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)")
+        pygame.display.set_caption("HARVESTER")
 
     def odom_callback(self, msg: Odometry):
         self.robot_x = msg.pose.pose.position.x
@@ -101,7 +92,7 @@ class HarvesterStateMachine(Node):
         
         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
+            gazebo_spawn_x, gazebo_spawn_y = -10.0, -2.0
 
             for wp in self.global_waypoints:
                 self.local_waypoints.append({
@@ -123,26 +114,21 @@ class HarvesterStateMachine(Node):
         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
+        l_x, l_y, l_yaw = -1.0, 0.5, -0.785398
 
-        # ... (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)
@@ -153,8 +139,6 @@ class HarvesterStateMachine(Node):
         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
@@ -163,7 +147,6 @@ class HarvesterStateMachine(Node):
             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))
             
@@ -177,14 +160,12 @@ class HarvesterStateMachine(Node):
         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
 
@@ -194,14 +175,12 @@ class HarvesterStateMachine(Node):
                 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()
@@ -233,7 +212,7 @@ class HarvesterStateMachine(Node):
             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.linear.x = 2.5  
                 cmd.angular.z = 0.8 * yaw_error  
             self.cmd_vel_pub.publish(cmd)
         else:
@@ -258,7 +237,7 @@ class HarvesterStateMachine(Node):
         
         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.get_logger().info("RETURNING TO GPS.")
             self.current_wp_idx += 1  
             self.current_state = self.STATE_GPS_DRIVE
             return
@@ -269,7 +248,6 @@ class HarvesterStateMachine(Node):
             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
 
@@ -293,31 +271,29 @@ class HarvesterStateMachine(Node):
         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)
+        # Siatka radarowa
         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)
+        # Rysowanie obrysu kombajnu
         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)
+        # Pozycja montażowa lidarow
         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"
+            status_text = "INITIALIZING..."
             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]"
@@ -325,7 +301,7 @@ class HarvesterStateMachine(Node):
             pygame.display.flip()
             return
 
-        # Rysowanie przeliczonych chmur punktów (Lewa ściana = zielona, Prawa = pomarańczowa)
+        # Rysowanie przeliczonych chmur punktów
         for x, y in zip(self.pts_L[0], self.pts_L[1]):
             px = cx - int(y * scale)
             py = cy - int(x * scale)
@@ -340,8 +316,8 @@ class HarvesterStateMachine(Node):
 
         # 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"DL:  {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))
+        self.screen.blit(font.render(f"DR:  {self.avg_right:.2f}m", True, (255, 165, 0)), (15, 55))
         pygame.display.flip()
 
 def main(args=None):

src/arm_moveit_config/config/six_axis_arm.srdf

@@ -32,4 +32,5 @@
     <disable_collisions link1="link_4" link2="link_5" reason="Adjacent"/>
     <disable_collisions link1="link_4" link2="link_6" reason="Never"/>
     <disable_collisions link1="link_5" link2="link_6" reason="Adjacent"/>
+    <disable_collisions link1="h_base_link" link2="arm::base_link" reason="Adjacent" />
 </robot>

src/arm_moveit_config/launch/sim.launch.py

@@ -63,7 +63,14 @@ def generate_launch_description():
         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'
+            # '/model/harvester_robot/joint/joint_1/cmd_pos@std_msgs/msg/Float64]gz.msgs.Double',
+            # 'cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
+            # 'model/harvester_robot/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',
+            # 'lidar_left@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+            # 'lidar_right@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
+            ],
+        remappings=[
+            ('/model/harvester_robot/odometry', '/odom')
         ],
         output='screen'
     )

src/world_description/corn_field.sdf

@@ -58,6 +58,48 @@
       </link>
     </model>
 
+    <include><name>C_0_0</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>0.02 -0.02 0 0 0 0.44</pose></include>
+    <include><name>C_0_1</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>0.65 0.03 0 0 0 1.57</pose></include>
+    <include><name>C_0_2</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>1.31 -0.01 0 0 0 3.12</pose></include>
+    <include><name>C_0_3</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>1.98 0.04 0 0 0 4.89</pose></include>
+    <include><name>C_0_4</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>2.62 -0.03 0 0 0 0.12</pose></include>
+    <include><name>C_0_5</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>3.25 0.01 0 0 0 2.33</pose></include>
+    <include><name>C_0_6</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>3.91 -0.02 0 0 0 5.67</pose></include>
+    <include><name>C_0_7</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>4.55 0.04 0 0 0 1.11</pose></include>
+    <include><name>C_0_8</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>5.21 -0.01 0 0 0 3.88</pose></include>
+    <include><name>C_0_9</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>5.85 0.02 0 0 0 2.44</pose></include>
+    <include><name>C_0_10</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>6.52 -0.03 0 0 0 1.05</pose></include>
+    <include><name>C_0_11</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>7.17 0.01 0 0 0 4.22</pose></include>
+    <include><name>C_0_12</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>7.83 -0.04 0 0 0 2.11</pose></include>
+    <include><name>C_0_13</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>8.48 0.02 0 0 0 5.89</pose></include>
+    <include><name>C_0_14</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>9.12 -0.01 0 0 0 0.34</pose></include>
+    <include><name>C_0_15</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>9.77 0.04 0 0 0 3.16</pose></include>
+    <include><name>C_0_16</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>10.41 -0.02 0 0 0 1.98</pose></include>
+    <include><name>C_0_17</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>11.06 0.03 0 0 0 4.75</pose></include>
+    <include><name>C_0_18</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>11.72 -0.05 0 0 0 0.88</pose></include>
+    <include><name>C_0_19</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>12.35 0.01 0 0 0 3.61</pose></include>
+
+    <include><name>C_1_0</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>-0.05 1.58 0 0 0 6.12</pose></include>
+    <include><name>C_1_1</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>0.62 1.64 0 0 0 0.88</pose></include>
+    <include><name>C_1_2</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>1.28 1.59 0 0 0 2.55</pose></include>
+    <include><name>C_1_3</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>1.92 1.62 0 0 0 4.12</pose></include>
+    <include><name>C_1_4</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>2.58 1.57 0 0 0 0.33</pose></include>
+    <include><name>C_1_5</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>3.21 1.65 0 0 0 1.99</pose></include>
+    <include><name>C_1_6</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>3.88 1.60 0 0 0 3.44</pose></include>
+    <include><name>C_1_7</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>4.52 1.63 0 0 0 5.21</pose></include>
+    <include><name>C_1_8</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>5.18 1.58 0 0 0 0.77</pose></include>
+    <include><name>C_1_9</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>5.81 1.61 0 0 0 2.15</pose></include>
+    <include><name>C_1_10</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>6.47 1.59 0 0 0 4.91</pose></include>
+    <include><name>C_1_11</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>7.12 1.63 0 0 0 1.34</pose></include>
+    <include><name>C_1_12</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>7.78 1.56 0 0 0 3.82</pose></include>
+    <include><name>C_1_13</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>8.43 1.62 0 0 0 5.02</pose></include>
+    <include><name>C_1_14</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>9.09 1.60 0 0 0 2.27</pose></include>
+    <include><name>C_1_15</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>9.73 1.64 0 0 0 0.41</pose></include>
+    <include><name>C_1_16</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>10.38 1.57 0 0 0 3.19</pose></include>
+    <include><name>C_1_17</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>11.03 1.61 0 0 0 1.15</pose></include>
+    <include><name>C_1_18</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>11.69 1.59 0 0 0 4.63</pose></include>
+    <include><name>C_1_19</name><uri>/home/marcin/arm_ws/src/world_description/models</uri><pose>12.33 1.62 0 0 0 2.85</pose></include>
+
     <light name='sun' type='directional'>
       <pose>0 0 10 0 0 0</pose>
       <cast_shadows>true</cast_shadows>
@@ -68,7 +110,7 @@
 
     <include>
       <uri>file:///home/marcin/arm_ws/src/world_description/harvester.sdf</uri>
-      <pose>-10 -10 3.3 0 0 0</pose>
+      <pose>-10 -2 3.3 0 0 0</pose>
     </include>
 
   </world>

src/world_description/harvester.sdf

@@ -6,7 +6,7 @@
       <render_engine>ogre2</render_engine> 
     </plugin>
 
-    <link name='base_link'>
+    <link name='h_base_link'>
       <inertial>
         <mass>150.0</mass> 
         <inertia>
@@ -34,10 +34,10 @@
     <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_fl' type='fixed'><parent>h_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_fr' type='fixed'><parent>h_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_bl' type='fixed'><parent>h_base_link</parent><child>axle_bl</child></joint>
-    <joint name='steer_br' type='fixed'><parent>base_link</parent><child>axle_br</child></joint>
+    <joint name='steer_br' type='fixed'><parent>h_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>
@@ -122,10 +122,10 @@
       <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_left_joint" type="fixed"><parent>h_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="lidar_right_joint" type="fixed"><parent>h_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="gps_joint" type="fixed"><parent>h_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="camera_joint" type="fixed"><parent>h_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>
@@ -139,8 +139,8 @@
     </include>
 
     <joint name="chassis_to_arm_joint" type="fixed">
-      <!-- <pose relative_to="base_link">0 0 -0.2 0 0 0</pose> -->
+      <!-- <pose relative_to="h_base_link">0 0 -0.2 0 0 0</pose> -->
-      <parent>base_link</parent>
+      <parent>h_base_link</parent>
       <child>arm::base_link</child>
     </joint>
 

src/world_description/models/corn.sdf

@@ -1,24 +1,267 @@
 <?xml version="1.0" ?>
 <sdf version="1.6">
-<model name="Corn">
+
+	<model name="Corn">
+
 		<static>true</static>
-  <link name="link">
+
-    <visual name="visual">
+		<!-- ========================= -->
+		<!-- STEM -->
+		<!-- ========================= -->
+
+		<link name="corn_stem">
+
+			<visual name="stem_visual">			  
 				<geometry>
 					<mesh>
-          <uri>meshes/scene.gltf</uri>
+						<uri>meshes/corn_stem.glb</uri>
 					</mesh>
 				</geometry>
 			</visual>
-    </link>
 
-    <collision name="lidar_blocker">
+			<collision name="stem_collision">
-      <pose>0 0 0.5 0 0 0</pose> <geometry>
+				<pose>0 0 1.0 0 0 0</pose>
+				<geometry>
 					<cylinder>
-          <radius>0.15</radius> <length>1.0</length>
+						<radius>0.025</radius>
+						<length>1.95</length>
 					</cylinder>
 				</geometry>
 			</collision>
 
-</model>
+			<inertial>
+				<mass>1.0</mass>
+				<inertia>
+					<ixx>0.35</ixx>
+					<iyy>0.001</iyy>
+					<izz>0.35</izz>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyz>0</iyz>
+				</inertia>
+			</inertial>
+
+		</link>
+
+		<!-- ========================= -->
+		<!-- COB 1 -->
+		<!-- ========================= -->
+
+		<link name="corn_cob1">
+
+			<pose>0.00 0.0 0.00 0.0 0 0</pose>
+
+			<visual name="visual">
+				<geometry>
+					<mesh>
+						<uri>meshes/corn_cob1.glb</uri>
+					</mesh>
+				</geometry>
+			</visual>
+
+			<collision name="collision">
+			  <pose>0 0.2 1.05 -0.80 0.0 0.0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.03</radius>
+            <length>0.4</length>
+          </cylinder>
+        </geometry>
+			</collision>
+
+			<inertial>
+				<mass>0.25</mass>
+				<inertia>
+					<ixx>0.001</ixx>
+					<iyy>0.001</iyy>
+					<izz>0.001</izz>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyz>0</iyz>
+				</inertia>
+			</inertial>
+
+		</link>
+
+		<!-- ========================= -->
+		<!-- COB 2 -->
+		<!-- ========================= -->
+
+		<link name="corn_cob2">
+
+			<pose>0.00 0.0 0.00 0.0 0 0</pose>
+
+			<visual name="visual">
+				<geometry>
+					<mesh>
+						<uri>meshes/corn_cob2.glb</uri>
+					</mesh>
+				</geometry>
+			</visual>
+
+			<collision name="collision">
+			  <pose>-0.13 -0.18 1.28 0.61 -0.75 -0.41</pose>
+				<geometry>
+          <cylinder>
+            <radius>0.03</radius>
+            <length>0.4</length>
+          </cylinder>-1.535890
+        </geometry>
+			</collision>
+
+			<inertial>
+				<mass>0.25</mass>
+				<inertia>
+					<ixx>0.001</ixx>
+					<iyy>0.001</iyy>
+					<izz>0.001</izz>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyz>0</iyz>
+				</inertia>
+			</inertial>
+
+		</link>
+
+		<!-- ========================= -->
+		<!-- COB 3 -->
+		<!-- ========================= -->
+
+		<link name="corn_cob3">
+
+			<pose>0.00 0.0 0.00 0.0 0 0</pose>
+
+			<visual name="visual">
+				<geometry>
+					<mesh>
+						<uri>meshes/corn_cob3.glb</uri>
+					</mesh>
+				</geometry>
+			</visual>
+
+			<collision name="collision">
+			  <pose>00.18 -0.04 1.65 0.31 -2.37 -0.16</pose>
+				<geometry>
+          <cylinder>
+            <radius>0.03</radius>
+            <length>0.4</length>
+          </cylinder>
+        </geometry>
+			</collision>
+
+			<inertial>
+				<mass>0.25</mass>
+				<inertia>
+					<ixx>0.001</ixx>
+					<iyy>0.001</iyy>
+					<izz>0.001</izz>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyz>0</iyz>
+				</inertia>
+			</inertial>
+
+		</link>
+
+		<!-- ========================= -->
+		<!-- COB 4 -->
+		<!-- ========================= -->
+
+		<link name="corn_cob4">
+
+			<pose>0.00 0.0 0.00 0.0 0 0</pose>
+
+			<visual name="visual">
+				<geometry>
+					<mesh>
+						<uri>meshes/corn_cob4.glb</uri>
+					</mesh>
+				</geometry>
+			</visual>
+
+			<collision name="collision">
+			  <pose>0 0 2.17 0 0 0.0</pose>
+				<geometry>
+          <cylinder>
+            <radius>0.03</radius>
+            <length>0.4</length>
+          </cylinder>
+        </geometry>
+			</collision>
+
+			<inertial>
+				<mass>0.25</mass>
+				<inertia>
+					<ixx>0.001</ixx>
+					<iyy>0.001</iyy>
+					<izz>0.001</izz>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyz>0</iyz>
+				</inertia>
+			</inertial>
+
+		</link><pose>0 0 1.0 0 0 0</pose>
+
+		<!-- ========================= -->
+		<!-- LEAF (VISUAL ONLY) -->
+		<!-- ========================= -->
+
+		<link name="corn_leaf">
+
+			<pose>0.00 0.0 0.00 0.0 0 0</pose>
+
+			<visual name="visual">
+			  <geometry>
+					<mesh>
+						<uri>meshes/corn_leaf.glb</uri>
+					</mesh>
+				</geometry>
+			</visual>
+
+			<inertial>
+				<mass>0.01</mass>
+				<inertia>
+					<ixx>0.00001</ixx>
+					<iyy>0.00001</iyy>
+					<izz>0.00001</izz>
+					<ixy>0</ixy>
+					<ixz>0</ixz>
+					<iyz>0</iyz>
+				</inertia>
+			</inertial>
+
+		</link>
+
+		<!-- ========================= -->
+		<!-- JOINTS -->
+		<!-- ========================= -->
+
+		<joint name="cob1_joint" type="fixed">
+			<parent>corn_stem</parent>
+			<child>corn_cob1</child>
+		</joint>
+
+		<joint name="cob2_joint" type="fixed">
+			<parent>corn_stem</parent>
+			<child>corn_cob2</child>
+		</joint>
+
+		<joint name="cob3_joint" type="fixed">
+			<parent>corn_stem</parent>
+			<child>corn_cob3</child>
+		</joint>
+
+		<joint name="cob4_joint" type="fixed">
+			<parent>corn_stem</parent>
+			<child>corn_cob4</child>
+		</joint>
+
+		<joint name="leaf_joint" type="fixed">
+			<parent>corn_stem</parent>
+			<child>corn_leaf</child>
+		</joint>
+
+	</model>
+
 </sdf>