__/gazebo_ros_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

reference frame

Browse Source
This commit is contained in:
Marcin M
2026-06-10 22:48:34 +02:00
parent e2283a7d6f
commit 65c9e81858
14 changed files with 358 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_harverster/CMakeLists.txt
+1
View File
@@ -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
+12 -12
View File
@@ -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/world_description/mission.py → src/arm_harverster/scripts/mission.py
Regular → Executable
+18 -42
View File
@@ -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 = ([], []) # Ściana lewa (zlewana z obu laserów)
self.pts_R = ([], []) # Ściana prawa (zlewana z obu laserów)
self.pts_L = ([], [])
self.pts_R = ([], [])
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}]
# PUNKTY DOCELOWE
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"DYSTANS OD PRAWEJ ŚCIANY: {self.avg_right:.2f}m", True, (255, 165, 0)), (15, 55))
self.screen.blit(font.render(f"DL: {self.avg_left:.2f}m", True, (100, 255, 100)), (15, 35))
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
+1
View File
@@ -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
+8 -1
View File
@@ -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
+43 -1
View File
@@ -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
+11 -11
View File
@@ -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_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>
<joint name='steer_fl' type='fixed'><parent>h_base_link</parent><child>axle_fl</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>h_base_link</parent><child>axle_bl</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_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="lidar_left_joint" type="fixed"><parent>h_base_link</parent><child>lidar_left_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>h_base_link</parent><child>gps_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> -->
<parent>base_link</parent>
<!-- <pose relative_to="h_base_link">0 0 -0.2 0 0 0</pose> -->
<parent>h_base_link</parent>
<child>arm::base_link</child>
</joint>
src/world_description/models/corn.sdf
+252 -9
View File
@@ -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">
<pose>0 0 0.5 0 0 0</pose> <geometry>
<collision name="stem_collision">
<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>
src/world_description/models/meshes/corn_cob1.glb
BIN
View File
Binary file not shown.
src/world_description/models/meshes/corn_cob2.glb
BIN
View File
Binary file not shown.
src/world_description/models/meshes/corn_cob3.glb
BIN
View File
Binary file not shown.
src/world_description/models/meshes/corn_cob4.glb
BIN
View File
Binary file not shown.
src/world_description/models/meshes/corn_leaf.glb
BIN
View File
Binary file not shown.
src/world_description/models/meshes/corn_stem.glb
BIN
View File
Binary file not shown.