Attached arm to rover, arm moving script added
This commit is contained in:
Marcin M
@@ -0,0 +1,3 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
@@ -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}
|
||||
)
|
||||
|
||||
|
||||
+235
@@ -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()
|
||||
@@ -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.y = 0.1
|
||||
target_pose.pose.position.z = 0.4
|
||||
target_pose.pose.orientation.w = 1.0 # Facing neutral forward
|
||||
target_pose.pose.position.x = -0.3
|
||||
target_pose.pose.position.y = 1.0
|
||||
target_pose.pose.position.z = 0.9
|
||||
|
||||
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()
|
||||
|
||||
@@ -7,19 +7,4 @@ moveit_setup_assistant_config:
|
||||
package_settings:
|
||||
author_name: marcin
|
||||
author_email: 113255@example.com
|
||||
generated_timestamp: 1780594876
|
||||
control_xacro:
|
||||
command:
|
||||
- position
|
||||
state:
|
||||
- position
|
||||
- velocity
|
||||
modified_urdf:
|
||||
xacros:
|
||||
- control_xacro
|
||||
control_xacro:
|
||||
command:
|
||||
- position
|
||||
state:
|
||||
- position
|
||||
- velocity
|
||||
generated_timestamp: 1781026475
|
||||
@@ -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
|
||||
@@ -1,4 +1,5 @@
|
||||
arm:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.005
|
||||
kinematics_solver_timeout: 0.05
|
||||
solve_type: Distance # Tells TRAC-IK to minimize deviations smoothly
|
||||
@@ -8,12 +8,12 @@ moveit_simple_controller_manager:
|
||||
|
||||
arm_controller:
|
||||
type: FollowJointTrajectory
|
||||
action_ns: follow_joint_trajectory
|
||||
default: True
|
||||
joints:
|
||||
- joint_1
|
||||
- joint_2
|
||||
- joint_3
|
||||
- joint_4
|
||||
- joint_5
|
||||
- joint_6
|
||||
- joint_6
|
||||
action_ns: follow_joint_trajectory
|
||||
default: True
|
||||
@@ -20,8 +20,7 @@ arm_controller:
|
||||
- joint_5
|
||||
- joint_6
|
||||
command_interfaces:
|
||||
- position
|
||||
[]
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
[]
|
||||
allow_nonzero_velocity_at_trajectory_end: true
|
||||
@@ -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
|
||||
@@ -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"/>
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
# This automatically loads the robot description (URDF/XACRO) and handles robot_state_publisher
|
||||
# 1. Include the standard MoveIt 2 move_group launch file (Delayed for stability)
|
||||
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
|
||||
# It listens to the /robot_description topic published by MoveIt's robot_state_publisher
|
||||
spawn_robot = Node(
|
||||
# 3. MODIFIED: Spawns the ENTIRE rover model from the master SDF file.
|
||||
# Because your SDF includes the arm internally, Gazebo builds the combined asset together.
|
||||
spawn_complete_rover = Node(
|
||||
package='ros_gz_sim',
|
||||
executable='create',
|
||||
arguments=[
|
||||
'-topic', 'robot_description',
|
||||
'-name', 'six_axis_arm',
|
||||
'-z', '0.0' # Adjust if your base needs to sit higher
|
||||
'-file', rover_sdf_path,
|
||||
'-name', 'harvester_robot', # Matches the model namespace inside your world bridges
|
||||
'-z', '0.1' # Clear the field ground mesh on entry
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# 5. Define ros2_control Spawners
|
||||
# Broadcaster for joint states (vital for MoveIt to know where the arm currently is)
|
||||
# 4. Define ros2_control Spawners
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
# Trajectory controller for sending trajectory paths to Gazebo
|
||||
arm_controller_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['arm_controller', '--controller-manager', '/controller_manager'],
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
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
|
||||
# This prevents ros2_control from crashing due to Gazebo not being ready yet
|
||||
# 6. Process the xacro file dynamically for the ROS 2 Kinematics Stack
|
||||
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,
|
||||
#move_group_launch,
|
||||
spawn_robot,
|
||||
spawn_complete_rover, # Spawns the single, complete vehicle
|
||||
robot_state_publisher,
|
||||
bridge,
|
||||
delay_joint_state_broadcaster,
|
||||
delay_arm_controller,
|
||||
robot_state_publisher_node,
|
||||
delay_move_group_launch,
|
||||
])
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
@@ -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"/>
|
||||
<geometry><cylinder radius="0.08" length="0.3"/></geometry>
|
||||
<origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.08" length="0.3"/></geometry>
|
||||
<origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.08" length="${0.3 * LENGTH_SCALE}"/></geometry>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="4.0" radius="0.08" length="0.3">
|
||||
<origin xyz="0 0 0.15" rpy="0 0 0"/>
|
||||
<xacro:cylinder_inertial mass="4.0" radius="0.08" length="${0.3 * LENGTH_SCALE}">
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.07" length="0.4"/></geometry>
|
||||
<origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.07" length="0.4"/></geometry>
|
||||
<origin xyz="0 0 ${0.2 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.07" length="${0.4 * LENGTH_SCALE}"/></geometry>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="3.5" radius="0.07" length="0.4">
|
||||
<origin xyz="0 0 0.2" rpy="0 0 0"/>
|
||||
<xacro:cylinder_inertial mass="3.5" radius="0.07" length="${0.4 * LENGTH_SCALE}">
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.06" length="0.3"/></geometry>
|
||||
<origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.06" length="0.3"/></geometry>
|
||||
<origin xyz="0 0 ${0.15 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.06" length="${0.3 * LENGTH_SCALE}"/></geometry>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="2.5" radius="0.06" length="0.3">
|
||||
<origin xyz="0 0 0.15" rpy="0 0 0"/>
|
||||
<xacro:cylinder_inertial mass="2.5" radius="0.06" length="${0.3 * LENGTH_SCALE}">
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.05" length="0.2"/></geometry>
|
||||
<origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.05" length="0.2"/></geometry>
|
||||
<origin xyz="0 0 ${0.1 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.05" length="${0.2 * LENGTH_SCALE}"/></geometry>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="1.8" radius="0.05" length="0.2">
|
||||
<origin xyz="0 0 0.1" rpy="0 0 0"/>
|
||||
<xacro:cylinder_inertial mass="1.8" radius="0.05" length="${0.2 * LENGTH_SCALE}">
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.04" length="0.1"/></geometry>
|
||||
<origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.04" length="0.1"/></geometry>
|
||||
<origin xyz="0 0 ${0.05 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.04" length="${0.1 * LENGTH_SCALE}"/></geometry>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="1.0" radius="0.04" length="0.1">
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
||||
<xacro:cylinder_inertial mass="1.0" radius="0.04" length="${0.1 * LENGTH_SCALE}">
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.03" length="0.04"/></geometry>
|
||||
<origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<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"/>
|
||||
<geometry><cylinder radius="0.03" length="0.04"/></geometry>
|
||||
<origin xyz="0 0 ${0.02 * LENGTH_SCALE}" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.03" length="${0.04 * LENGTH_SCALE}"/></geometry>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="0.5" radius="0.03" length="0.04">
|
||||
<origin xyz="0 0 0.02" rpy="0 0 0"/>
|
||||
<xacro:cylinder_inertial mass="0.5" radius="0.03" length="${0.04 * LENGTH_SCALE}">
|
||||
<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>
|
||||
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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()
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 262 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 10 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 10 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 10 KiB |
@@ -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>
|
||||
@@ -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/)
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 693 KiB |
Binary file not shown.
@@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
BIN
Binary file not shown.
|
After Width: | Height: | Size: 176 KiB |
@@ -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>
|
||||
Reference in New Issue
Block a user