diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1345657 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build/ +install/ +log/ diff --git a/src/arm_harverster/CMakeLists.txt b/src/arm_harverster/CMakeLists.txt index b1bb59e..ac153c3 100644 --- a/src/arm_harverster/CMakeLists.txt +++ b/src/arm_harverster/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED) # Installs your Python script file and marks it executable install(PROGRAMS scripts/move_to_point.py + scripts/arm_point_controller.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/arm_harverster/scripts/arm_point_controller.py b/src/arm_harverster/scripts/arm_point_controller.py new file mode 100755 index 0000000..52492b1 --- /dev/null +++ b/src/arm_harverster/scripts/arm_point_controller.py @@ -0,0 +1,235 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +import math +import time + +# ROS 2 Message, Action, and Service Imports +from moveit_msgs.action import MoveGroup, ExecuteTrajectory +from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume, RobotState +from moveit_msgs.srv import GetCartesianPath, GetPositionFK +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import PoseStamped, Pose +from sensor_msgs.msg import JointState + +class CameraArmController(Node): + def __init__(self): + super().__init__('camera_arm_controller') + + # 1. Action Client for normal point-to-point planning + self._action_client = ActionClient(self, MoveGroup, '/move_action') + self.get_logger().info("Waiting for MoveIt action server...") + self._action_client.wait_for_server() + + # 2. Action Client for running raw pre-calculated paths + self._execute_client = ActionClient(self, ExecuteTrajectory, '/execute_trajectory') + self.get_logger().info("Waiting for execution action server...") + self._execute_client.wait_for_server() + + # 3. Service Client for calculating straight horizontal camera paths + self._cartesian_client = self.create_client(GetCartesianPath, '/compute_cartesian_path') + self.get_logger().info("Connecting to Cartesian path service...") + self._cartesian_client.wait_for_service() + + # 4. NEW: Service Client for fetching live end-effector coordinates (Forward Kinematics) + self._fk_client = self.create_client(GetPositionFK, '/compute_fk') + self.get_logger().info("Connecting to Forward Kinematics service...") + self._fk_client.wait_for_service() + + # 5. Subscriber to listen to current joint states (Required to compute current position) + self.current_joint_state = None + self._joint_sub = self.create_subscription( + JointState, + '/joint_states', + self.joint_state_callback, + 10 + ) + + self.get_logger().info("🚀 Automated Camera Arm Pipeline Fully Operational!") + + def joint_state_callback(self, msg): + """Saves current joint angles to calculate position on the fly""" + self.current_joint_state = msg + + def get_horizontal_camera_quaternion(self): + """Calculates a 90-degree pitch offset to keep the camera level with the horizon""" + pitch_angle = math.radians(-90.0) + q = Pose().orientation + q.x = 0.0 + q.y = math.sin(pitch_angle / 2.0) + q.z = 0.0 + q.w = math.cos(pitch_angle / 2.0) + return q + + def get_current_end_effector_pose(self): + """Queries the FK service to find exactly where the camera is right now""" + # Wait until we receive at least one joint state message from the robot + while self.current_joint_state is None and rclpy.ok(): + self.get_logger().info("Waiting for initial joint states...") + rclpy.spin_once(self, timeout_sec=0.1) + + req = GetPositionFK.Request() + req.header.frame_id = "world" + req.fk_link_names = ["tool_link"] # Name of the tip frame to track + + # Pack current joint configuration into the request + robot_state = RobotState() + robot_state.joint_state = self.current_joint_state + req.robot_state = robot_state + + # Call the service synchronously + future = self._fk_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + + res = future.result() + if res and res.error_code.val == 1: # 1 = SUCCESS + return res.pose_stamped[0].pose + else: + self.get_logger().error("Failed to calculate Forward Kinematics!") + return None + + def send_pt_to_pt_goal(self, x, y, z): + """Moves the camera to a specific 3D coordinate while forcing it to be horizontal""" + self.get_logger().info(f"Planning Point-to-Point Move to: X={x}, Y={y}, Z={z}") + + goal_msg = MoveGroup.Goal() + goal_msg.request.group_name = "arm" + goal_msg.request.allowed_planning_time = 10.0 + goal_msg.request.num_planning_attempts = 15 + goal_msg.request.pipeline_id = "ompl" + + target_pose = PoseStamped() + target_pose.header.frame_id = "world" + target_pose.pose.position.x = float(x) + target_pose.pose.position.y = float(y) + target_pose.pose.position.z = float(z) + target_pose.pose.orientation = self.get_horizontal_camera_quaternion() + + position_constraint = PositionConstraint() + position_constraint.header.frame_id = "world" + position_constraint.link_name = "tool_link" + + box = SolidPrimitive() + box.type = SolidPrimitive.BOX + box.dimensions = [0.03, 0.03, 0.03] + + volume = BoundingVolume() + volume.primitives.append(box) + + box_pose = Pose() + box_pose.position = target_pose.pose.position + box_pose.orientation.w = 1.0 + volume.primitive_poses.append(box_pose) + + position_constraint.constraint_region = volume + position_constraint.weight = 1.0 + + orient_constraint = OrientationConstraint() + orient_constraint.header.frame_id = "world" + orient_constraint.link_name = "tool_link" + orient_constraint.orientation = target_pose.pose.orientation + + orient_constraint.absolute_x_axis_tolerance = 0.15 + orient_constraint.absolute_y_axis_tolerance = 0.15 + orient_constraint.absolute_z_axis_tolerance = 3.14 + orient_constraint.weight = 1.0 + + goal_constraints = Constraints() + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orient_constraint) + goal_msg.request.goal_constraints.append(goal_constraints) + + send_goal_future = self._action_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(self, send_goal_future) + + goal_handle = send_goal_future.result() + if not goal_handle.accepted: + self.get_logger().error("❌ Point-to-Point goal rejected by MoveIt!") + return False + + self.get_logger().info("Goal accepted! Executing path...") + get_result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, get_result_future) + return True + + def execute_straight_camera_sweep(self, distance=0.2): + """ + AUTOMATED NO-XYZ METHOD: Fetches live position from the simulation + and advances the camera straight forward parallel to the ground. + """ + # 1. Fetch live position automatically + current_pose = self.get_current_end_effector_pose() + if current_pose is None: + self.get_logger().error("Cannot track sweep because current pose is unknown.") + return False + + current_x = current_pose.position.x + current_y = current_pose.position.y + current_z = current_pose.position.z + + self.get_logger().info(f"📍 Detected current position: X={current_x:.3f}, Y={current_y:.3f}, Z={current_z:.3f}") + self.get_logger().info(f"Calculating straight crawl: advancing +{distance}m forward along X...") + + req = GetCartesianPath.Request() + req.group_name = "arm" + req.header.frame_id = "world" + req.start_state.is_diff = True + + # Target waypoint calculated using live variables + target_waypoint = Pose() + target_waypoint.position.x = current_x + distance + target_waypoint.position.y = current_y + target_waypoint.position.z = current_z + target_waypoint.orientation = self.get_horizontal_camera_quaternion() + + req.waypoints = [target_waypoint] + req.max_step = 0.01 + req.jump_threshold = 0.0 + + # Call Cartesian planner service + future = self._cartesian_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + + res = future.result() + if res.fraction < 1.0: + self.get_logger().warn(f"⚠️ Only calculated {res.fraction*100:.1f}% of path due to link extension limits.") + if res.fraction < 0.5: + return False + + self.get_logger().info("Executing calculated straight line Cartesian trajectory...") + execute_goal = ExecuteTrajectory.Goal() + execute_goal.trajectory = res.solution + + send_goal_future = self._execute_client.send_goal_async(execute_goal) + rclpy.spin_until_future_complete(self, send_goal_future) + + goal_handle = send_goal_future.result() + if goal_handle and goal_handle.accepted: + get_result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, get_result_future) + self.get_logger().info("✅ Straight camera sweep complete!") + return True + else: + self.get_logger().error("❌ Trajectory execution rejected.") + return False + +def main(args=None): + rclpy.init(args=args) + node = CameraArmController() + + # 1. Send to starting exploration zone + success = node.send_pt_to_pt_goal(0.6, 0.0, 0.5) + + if success: + time.sleep(2.0) + + # 2. Call the sweep without passing any XYZ parameters! + # It handles the math completely internally. + node.execute_straight_camera_sweep(distance=0.2) + + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/arm_harverster/scripts/move_to_point.py b/src/arm_harverster/scripts/move_to_point.py index f64d551..5358200 100755 --- a/src/arm_harverster/scripts/move_to_point.py +++ b/src/arm_harverster/scripts/move_to_point.py @@ -29,10 +29,14 @@ class ArmController(Node): # 2. Define our target pose target_pose = PoseStamped() target_pose.header.frame_id = "world" - target_pose.pose.position.x = 0.2 - target_pose.pose.position.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() diff --git a/src/arm_moveit_config/.setup_assistant b/src/arm_moveit_config/.setup_assistant index eff8475..dfeb145 100644 --- a/src/arm_moveit_config/.setup_assistant +++ b/src/arm_moveit_config/.setup_assistant @@ -7,19 +7,4 @@ moveit_setup_assistant_config: package_settings: author_name: marcin author_email: 113255@example.com - generated_timestamp: 1780594876 - control_xacro: - command: - - position - state: - - position - - velocity - modified_urdf: - xacros: - - control_xacro - control_xacro: - command: - - position - state: - - position - - velocity \ No newline at end of file + generated_timestamp: 1781026475 \ No newline at end of file diff --git a/src/arm_moveit_config/config/joint_limits.yaml b/src/arm_moveit_config/config/joint_limits.yaml index 3ae912e..2367bfa 100644 --- a/src/arm_moveit_config/config/joint_limits.yaml +++ b/src/arm_moveit_config/config/joint_limits.yaml @@ -12,29 +12,29 @@ joint_limits: has_velocity_limits: true max_velocity: 2.5 has_acceleration_limits: true - max_acceleration: 5.0 + max_acceleration: 10.0 joint_2: has_velocity_limits: true max_velocity: 2.0 has_acceleration_limits: true - max_acceleration: 5.0 + max_acceleration: 10.0 joint_3: has_velocity_limits: true max_velocity: 2.5 has_acceleration_limits: true - max_acceleration: 5.0 + max_acceleration: 10.0 joint_4: has_velocity_limits: true max_velocity: 3.0 has_acceleration_limits: true - max_acceleration: 5.0 + max_acceleration: 10.0 joint_5: has_velocity_limits: true max_velocity: 3.0 has_acceleration_limits: true - max_acceleration: 5.0 + max_acceleration: 10.0 joint_6: has_velocity_limits: true max_velocity: 4.0 has_acceleration_limits: true - max_acceleration: 5.0 \ No newline at end of file + max_acceleration: 10.0 \ No newline at end of file diff --git a/src/arm_moveit_config/config/kinematics.yaml b/src/arm_moveit_config/config/kinematics.yaml index 2f6bb96..e6c2f89 100644 --- a/src/arm_moveit_config/config/kinematics.yaml +++ b/src/arm_moveit_config/config/kinematics.yaml @@ -1,4 +1,5 @@ arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.0050000000000000001 - kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file + 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 \ No newline at end of file diff --git a/src/arm_moveit_config/config/moveit_controllers.yaml b/src/arm_moveit_config/config/moveit_controllers.yaml index 09dfa3d..7bc4342 100644 --- a/src/arm_moveit_config/config/moveit_controllers.yaml +++ b/src/arm_moveit_config/config/moveit_controllers.yaml @@ -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 \ No newline at end of file + - joint_6 + action_ns: follow_joint_trajectory + default: True \ No newline at end of file diff --git a/src/arm_moveit_config/config/ros2_controllers.yaml b/src/arm_moveit_config/config/ros2_controllers.yaml index fec9baf..3d8a332 100644 --- a/src/arm_moveit_config/config/ros2_controllers.yaml +++ b/src/arm_moveit_config/config/ros2_controllers.yaml @@ -20,8 +20,7 @@ arm_controller: - joint_5 - joint_6 command_interfaces: - - position + [] state_interfaces: - - position - - velocity + [] allow_nonzero_velocity_at_trajectory_end: true \ No newline at end of file diff --git a/src/arm_moveit_config/config/sensors_3d.yaml b/src/arm_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..5d90e51 --- /dev/null +++ b/src/arm_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,23 @@ +sensors: + - kinect_pointcloud + - kinect_depthimage +kinect_pointcloud: + filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater +kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 \ No newline at end of file diff --git a/src/arm_moveit_config/config/six_axis_arm.srdf b/src/arm_moveit_config/config/six_axis_arm.srdf index c700bad..367b070 100644 --- a/src/arm_moveit_config/config/six_axis_arm.srdf +++ b/src/arm_moveit_config/config/six_axis_arm.srdf @@ -15,11 +15,13 @@ - + + + diff --git a/src/arm_moveit_config/launch/move_group.launch.py b/src/arm_moveit_config/launch/move_group.launch.py index 4c8410e..6ff9f99 100644 --- a/src/arm_moveit_config/launch/move_group.launch.py +++ b/src/arm_moveit_config/launch/move_group.launch.py @@ -3,6 +3,13 @@ from moveit_configs_utils.launches import generate_move_group_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs() + #moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs() + moveit_config = ( + MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config") + # Overrides the parameters dictionary layout natively + .to_moveit_configs() + ) + + # Manually append use_sim_time to the master parameter dictionaries moveit_config.robot_description_kinematics["use_sim_time"] = True return generate_move_group_launch(moveit_config) diff --git a/src/arm_moveit_config/launch/sim.launch.py b/src/arm_moveit_config/launch/sim.launch.py index 86079dc..02d6643 100644 --- a/src/arm_moveit_config/launch/sim.launch.py +++ b/src/arm_moveit_config/launch/sim.launch.py @@ -1,74 +1,94 @@ import os +import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.actions import IncludeLaunchDescription, RegisterEventHandler, TimerAction from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from launch.actions import IncludeLaunchDescription, TimerAction -from launch.launch_description_sources import PythonLaunchDescriptionSource + +# Absolute and relative path variables +world_path = './src/world_description/corn_field.sdf' +arm_urdf_path = '/home/marcin/arm_ws/robotarm_description/urdf/robot_arm.urdf' +rover_sdf_path = '/home/marcin/arm_ws/src/world_description/harvester.sdf' def generate_launch_description(): - # 1. Define package directories (Adjust names if yours differ) moveit_config_pkg = get_package_share_directory('arm_moveit_config') - # 2. Include the standard MoveIt 2 move_group launch file - # 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, ]) \ No newline at end of file diff --git a/src/arm_moveit_config/package.xml b/src/arm_moveit_config/package.xml index 1836b40..79a6da2 100644 --- a/src/arm_moveit_config/package.xml +++ b/src/arm_moveit_config/package.xml @@ -43,7 +43,6 @@ rviz_default_plugins tf2_ros warehouse_ros_mongo - xacro diff --git a/src/robotarm_description/urdf/robot_arm.urdf b/src/robotarm_description/urdf/robot_arm.urdf new file mode 100644 index 0000000..b0b3f3d --- /dev/null +++ b/src/robotarm_description/urdf/robot_arm.urdf @@ -0,0 +1,269 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + /home/marcin/arm_ws/install/robotarm_description/share/robotarm_description/config/ros2_controllers.yaml + + + diff --git a/src/robotarm_description/urdf/robot_arm.urdf.xacro b/src/robotarm_description/urdf/robot_arm.urdf.xacro index ecaf674..c961ae3 100644 --- a/src/robotarm_description/urdf/robot_arm.urdf.xacro +++ b/src/robotarm_description/urdf/robot_arm.urdf.xacro @@ -3,6 +3,8 @@ + + @@ -18,13 +20,13 @@ - + @@ -52,23 +54,23 @@ - - + + - - + + - - + + - + @@ -76,23 +78,23 @@ - - + + - - + + - - + + - + @@ -100,23 +102,23 @@ - - + + - - + + - - + + - + @@ -124,23 +126,23 @@ - - + + - - + + - - + + - + @@ -148,23 +150,23 @@ - - + + - - + + - - + + - + @@ -172,16 +174,16 @@ - - + + - - + + - - + + @@ -189,7 +191,7 @@ - + @@ -247,7 +249,7 @@ - + $(find robotarm_description)/config/ros2_controllers.yaml diff --git a/src/world_description/corn_field.sdf b/src/world_description/corn_field.sdf new file mode 100644 index 0000000..dc3481f --- /dev/null +++ b/src/world_description/corn_field.sdf @@ -0,0 +1,75 @@ + + + + 0.001 + 1 + 1000 + + + + + + 0 0 -9.8 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + + + 0.6 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 100 100 + + + + 0.2 0.15 0.1 1 + 0.25 0.18 0.12 1 + 0 0 0 1 + + + + + + + 0 0 10 0 0 0 + true + 1 + -0.5 0.1 -0.9 + 0.8 0.8 0.8 1 + + + + file:///home/marcin/arm_ws/src/world_description/harvester.sdf + -10 -10 3.3 0 0 0 + + + + \ No newline at end of file diff --git a/src/world_description/harvester.sdf b/src/world_description/harvester.sdf new file mode 100644 index 0000000..34dfd48 --- /dev/null +++ b/src/world_description/harvester.sdf @@ -0,0 +1,156 @@ + + + + 0 0 3.3 0 0 0 + + ogre2 + + + + + 150.0 + + 128.500 + 200.50 + 328.0 + + + + 0 1.4 0 0 0 0 4.0 0.4 0.2 + + + 0 -1.4 0 0 0 0 4.0 0.4 0.2 + + + 0 0 0.5 0 0 0 4.0 2.4 0.1 + + + 4.0 3.2 0.2 0.1 0.3 0.6 10.1 0.3 0.6 1 + + + + 1.8 1.4 -1.6 0 0 0 0.2 0.2 3.00.2 0.2 0.2 1 + 1.8 -1.4 -1.6 0 0 0 0.2 0.2 3.00.2 0.2 0.2 1 + -1.8 1.4 -1.6 0 0 0 0.2 0.2 3.00.2 0.2 0.2 1 + -1.8 -1.4 -1.6 0 0 0 0.2 0.2 3.00.2 0.2 0.2 1 + + base_linkaxle_fl + base_linkaxle_fr + base_linkaxle_bl + base_linkaxle_br + + + 1.8 1.4 -2.9 1.5707 0 0 10.00.1000.100.1 + 0.40.1 + 1.00.41 0 00.00.01000000.0100.00.001 + + 0.40.10 0 0 1 + + + + 1.8 -1.4 -2.9 1.5707 0 0 10.00.1000.100.1 + 0.40.1 + 1.00.41 0 00.00.01000000.0100.00.001 + + 0.40.10 0 0 1 + + + + -1.8 1.4 -2.9 1.5707 0 0 10.00.1000.100.1 + 0.40.1 + 1.00.41 0 00.00.01000000.0100.00.001 + + 0.40.10 0 0 1 + + + + -1.8 -1.4 -2.9 1.5707 0 0 10.00.1000.100.1 + 0.40.1 + 1.00.41 0 00.00.01000000.0100.00.001 + + 0.40.10 0 0 1 + + + + -1.0 0.5 -2.4 0 0 -0.785398 + 0.10.00010.00010.0001 + 0.1 0.1 0.10 1 0 1 + + 0 0 0 0 0 0 + lidar_left_link + lidar_left + 10 + + + 1801-0.7853980.785398 + + 0.08100.01 + + + + + + -1.0 -0.5 -2.4 0 0 0.785398 + 0.10.00010.00010.0001 + 0.1 0.1 0.11 0.5 0 1 + + 0 0 0 0 0 0 + lidar_right_link + lidar_right + 10 + + + 1801-0.7853980.785398 + + 0.08100.01 + + + + + + 1.95 0 -1 0 0.15 0 0.1 0.00010.00010.0001 + 0.1 0.1 0.1 0.1 0.1 0.1 1 + + 1.047 19201080 0.1100 + 1 30 true camera + + + + + 0 0 0.2 0 0 0 0.1 0.00010.00010.0001 + 0.10.05 0.8 0.1 0.1 1 + 1 10 gps + + + base_linklidar_left_link + base_linklidar_right_link + base_linkgps_link + base_linkcamera_link + + axle_flwheel_fl0 1 010000 + axle_frwheel_fr0 1 010000 + axle_blwheel_bl0 1 010000 + axle_brwheel_br0 1 010000 + + + file:///home/marcin/arm_ws/src/robotarm_description/urdf/robot_arm.urdf + arm + 0 0 -0.1 3.14159 0 0 + + + + + base_link + arm::base_link + + + + wheel_fl_jointwheel_bl_joint + wheel_fr_jointwheel_br_joint + 2.8 0.4 500.0 + 5.05.0 + /cmd_vel + + + + \ No newline at end of file diff --git a/src/world_description/mission.py b/src/world_description/mission.py new file mode 100644 index 0000000..e9e68b9 --- /dev/null +++ b/src/world_description/mission.py @@ -0,0 +1,361 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +import math +import time +import sys +import numpy as np + +try: + import pygame +except ImportError: + print("Błąd: Brak biblioteki pygame. Zainstaluj wpisując: pip3 install pygame") + sys.exit(1) + +class HarvesterStateMachine(Node): + def __init__(self): + super().__init__('harvester_sm_node') + + # --- KONFIGURACJA TEMATÓW --- + odom_topic_name = '/odom' + cmd_vel_topic_name = '/cmd_vel' + + qos_profile = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=10 + ) + + # --- PUBLISHERS & SUBSCRIBERS --- + self.cmd_vel_pub = self.create_publisher(Twist, cmd_vel_topic_name, 10) + self.odom_sub = self.create_subscription(Odometry, odom_topic_name, self.odom_callback, qos_profile) + + # Subskrypcja dwóch krzyżujących się LiDARów zgodnie z Twoim planem + self.lidar_left_sub = self.create_subscription(LaserScan, '/lidar_left', self.lidar_left_callback, qos_profile) + self.lidar_right_sub = self.create_subscription(LaserScan, '/lidar_right', self.lidar_right_callback, qos_profile) + + # --- MASZYNA STANÓW --- + self.STATE_INITIALIZING = "INITIALIZING" + self.STATE_GPS_DRIVE = "GPS_DRIVE" + self.STATE_LIDAR_DRIVE = "LIDAR_DRIVE" + self.STATE_HARVESTING = "HARVESTING" + self.STATE_MISSION_DONE = "MISSION_DONE" + self.current_state = self.STATE_INITIALIZING + + # --- ZMIENNE ODOMETRII --- + self.robot_x, self.robot_y, self.robot_yaw = 0.0, 0.0, 0.0 + self.has_odom = False + self.start_offset_x, self.start_offset_y = 0.0, 0.0 + + # --- CHMURY PUNKTÓW PO FUZJI (W UKŁADZIE ROBOTA) --- + self.pts_L = ([], []) # Ściana lewa (zlewana z obu laserów) + self.pts_R = ([], []) # Ściana prawa (zlewana z obu laserów) + + self.avg_left = 0.0 + self.avg_right = 0.0 + self.end_of_row_detected = False + + # Bufory na surowe wiadomości ROS2 + self.latest_left_scan = None + self.latest_right_scan = None + + # --- LOGIKA PRZEJAZDU --- + self.lidar_start_x, self.lidar_start_y = 0.0, 0.0 + self.lidar_enter_time = 0.0 + self.harvesting_done_in_this_row = False + self.harvest_trigger_distance = 6.0 + + # --- PUNKTY DOCELOWE --- + self.global_waypoints = [{"x": -3.0, "y": 0.8}, {"x": -3.0, "y": 2.4}] + self.local_waypoints = [] + self.current_wp_idx = 0 + self.position_reached = False + + # --- PARAMETRY REGULACJI --- + self.target_pos_tolerance = 0.4 + self.target_heading_tolerance = 0.05 + self.lidar_speed = 0.25 + self.lidar_kp = 0.35 + self.deadzone = 0.08 + + self.control_timer = self.create_timer(0.1, self.control_loop) + + # --- PYGAME WIZUALIZACJA --- + pygame.init() + self.win_size = 600 + self.screen = pygame.display.set_mode((self.win_size, self.win_size)) + pygame.display.set_caption("Kombajn: Lokalna Fuzja Układu Współrzędnych (Cross-Eye)") + + def odom_callback(self, msg: Odometry): + self.robot_x = msg.pose.pose.position.x + self.robot_y = msg.pose.pose.position.y + + q = msg.pose.pose.orientation + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z) + self.robot_yaw = math.atan2(siny_cosp, cosy_cosp) + + if not self.has_odom: + self.start_offset_x, self.start_offset_y = self.robot_x, self.robot_y + gazebo_spawn_x, gazebo_spawn_y = -10.0, -10.0 + + for wp in self.global_waypoints: + self.local_waypoints.append({ + "x": self.start_offset_x + (wp["x"] - gazebo_spawn_x), + "y": self.start_offset_y + (wp["y"] - gazebo_spawn_y) + }) + self.has_odom = True + + def lidar_left_callback(self, msg: LaserScan): + self.latest_left_scan = msg + + def lidar_right_callback(self, msg: LaserScan): + self.latest_right_scan = msg + + def process_lidar_fusion(self): + if not self.latest_left_scan or not self.latest_right_scan: + return + + x_left_wall, y_left_wall = [], [] + x_right_wall, y_right_wall = [], [] + + # --- SENSOR 1: LEWY LIDAR (Nowe pozycje!) --- + l_msg = self.latest_left_scan + l_x, l_y, l_yaw = -1.0, 0.5, -0.785398 # <--- TUTAJ ZMIANA + + # ... (reszta pętli dla lewego lasera bez zmian) ... + + for i, distance in enumerate(l_msg.ranges): + if math.isinf(distance) or math.isnan(distance) or distance < 3.0 or distance > l_msg.range_max: + continue + angle = l_msg.angle_min + i * l_msg.angle_increment + + # Pozycja punktu względem samego sensora + xs = distance * math.cos(angle) + ys = distance * math.sin(angle) + + # Rzutowanie (Transformacja macierzowa) na układ robota (środek ramy) + xr = l_x + (xs * math.cos(l_yaw) - ys * math.sin(l_yaw)) + yr = l_y + (xs * math.sin(l_yaw) + ys * math.cos(l_yaw)) + + # Segregacja: jeśli punkt wylądował po lewej stronie robota (Yr > 0), to lewa ściana, inaczej prawa + if yr > 0.0: + x_left_wall.append(xr) + y_left_wall.append(yr) + else: + x_right_wall.append(xr) + y_right_wall.append(yr) + + r_msg = self.latest_right_scan + r_x, r_y, r_yaw = -1.0, -0.5, 0.785398 # <--- TUTAJ ZMIANA + + # ... (reszta pętli dla prawego lasera bez zmian) ... + + for i, distance in enumerate(r_msg.ranges): + if math.isinf(distance) or math.isnan(distance) or distance < 3.0 or distance > r_msg.range_max: + continue + angle = r_msg.angle_min + i * r_msg.angle_increment + + xs = distance * math.cos(angle) + ys = distance * math.sin(angle) + + # Rzutowanie na układ robota + xr = r_x + (xs * math.cos(r_yaw) - ys * math.sin(r_yaw)) + yr = r_y + (xs * math.sin(r_yaw) + ys * math.cos(r_yaw)) + + if yr > 0.0: + x_left_wall.append(xr) + y_left_wall.append(yr) + else: + x_right_wall.append(xr) + y_right_wall.append(yr) + + self.pts_L = (x_left_wall, y_left_wall) + self.pts_R = (x_right_wall, y_right_wall) + + # Wyliczanie średnich odległości bocznych korytarza dla regulatora PID + min_points = 4 + if len(y_left_wall) < min_points and len(y_right_wall) < min_points: + self.end_of_row_detected = True + self.avg_left, self.avg_right = 1.4, 1.4 + else: + self.end_of_row_detected = False + # Średnia odległość punktów od osi podłużnej robota (Y=0) + self.avg_left = sum(y_left_wall) / len(y_left_wall) if y_left_wall else 1.4 + self.avg_right = abs(sum(y_right_wall) / len(y_right_wall)) if y_right_wall else 1.4 + + def control_loop(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + pygame.quit() + sys.exit() + + # Dynamiczny start w oparciu o zebrany komplet tematów z mostka + if self.current_state == self.STATE_INITIALIZING: + if self.has_odom and self.latest_left_scan is not None and self.latest_right_scan is not None: + self.current_state = self.STATE_GPS_DRIVE + self.draw_pygame_window() + return + + # Wykonaj fuzję współrzędnych lokalnych przed podjęciem decyzji o ruchu + self.process_lidar_fusion() + + if self.current_state == self.STATE_GPS_DRIVE: self.execute_gps_drive() + elif self.current_state == self.STATE_LIDAR_DRIVE: self.execute_lidar_drive() + elif self.current_state == self.STATE_HARVESTING: self.execute_harvesting() + elif self.current_state == self.STATE_MISSION_DONE: self.stop_robot() + + self.draw_pygame_window() + + def execute_gps_drive(self): + if self.current_wp_idx >= len(self.local_waypoints): + self.current_state = self.STATE_MISSION_DONE + return + + target = self.local_waypoints[self.current_wp_idx] + cmd = Twist() + + if not self.position_reached: + dx, dy = target["x"] - self.robot_x, target["y"] - self.robot_y + distance = math.sqrt(dx**2 + dy**2) + desired_yaw = math.atan2(dy, dx) + yaw_error = math.atan2(math.sin(desired_yaw - self.robot_yaw), math.cos(desired_yaw - self.robot_yaw)) + + if distance < self.target_pos_tolerance: + self.stop_robot() + self.position_reached = True + return + + if abs(yaw_error) > 0.15: + cmd.angular.z = max(min(0.6 * yaw_error, 0.5), -0.5) + else: + cmd.linear.x = 0.4 + cmd.angular.z = 0.8 * yaw_error + self.cmd_vel_pub.publish(cmd) + else: + yaw_error = math.atan2(math.sin(0.0 - self.robot_yaw), math.cos(0.0 - self.robot_yaw)) + if abs(yaw_error) < self.target_heading_tolerance: + self.stop_robot() + self.position_reached = False + self.lidar_start_x, self.lidar_start_y = self.robot_x, self.robot_y + self.lidar_enter_time = time.time() + self.harvesting_done_in_this_row = False + self.current_state = self.STATE_LIDAR_DRIVE + return + + cmd.angular.z = max(min(0.5 * yaw_error, 0.4), -0.4) + if 0 < cmd.angular.z < 0.15: cmd.angular.z = 0.15 + if -0.15 < cmd.angular.z < 0: cmd.angular.z = -0.15 + self.cmd_vel_pub.publish(cmd) + + def execute_lidar_drive(self): + cmd = Twist() + time_in_lidar = time.time() - self.lidar_enter_time + + if self.end_of_row_detected and time_in_lidar > 6.0: + self.stop_robot() + self.get_logger().info("Wyjazd z korytarza. Powrót do nawigacji GPS.") + self.current_wp_idx += 1 + self.current_state = self.STATE_GPS_DRIVE + return + + dist_traveled = math.sqrt((self.robot_x - self.lidar_start_x)**2 + (self.robot_y - self.lidar_start_y)**2) + if dist_traveled >= self.harvest_trigger_distance and not self.harvesting_done_in_this_row: + self.stop_robot() + self.current_state = self.STATE_HARVESTING + return + + # Logika centrowania oparta o zunifikowane odległości boczne Y po fuzji + diff = self.avg_left - self.avg_right + cmd.linear.x = self.lidar_speed + + if abs(diff) < self.deadzone: + cmd.angular.z = 0.0 + else: + cmd.angular.z = self.lidar_kp * diff + + cmd.angular.z = max(min(cmd.angular.z, 0.2), -0.2) + self.cmd_vel_pub.publish(cmd) + + def execute_harvesting(self): + self.stop_robot() + time.sleep(2.0) + self.harvesting_done_in_this_row = True + self.current_state = self.STATE_LIDAR_DRIVE + + def stop_robot(self): + cmd = Twist() + cmd.linear.x, cmd.angular.z = 0.0, 0.0 + self.cmd_vel_pub.publish(cmd) + + def draw_pygame_window(self): + """Wizualizacja zrzutowanych punktów w lokalnej macierzy robota""" + self.screen.fill((12, 16, 24)) + + # Środek okna reprezentuje geometryczny środek kombajnu + cx, cy = self.win_size // 2, self.win_size // 2 + 50 + scale = 55.0 + + # Siatka radarowa (okręgi co 1 metr) + for r in range(1, 8): + pygame.draw.circle(self.screen, (25, 35, 45), (cx, cy), int(r * scale), 1) + + # Rysowanie obrysu kombajnu (SDF: długość 4.0, szerokość 3.2) + robot_w = int(3.2 * scale) + robot_h = int(4.0 * scale) + rx = cx - robot_w // 2 + ry = cy - robot_h // 2 + pygame.draw.rect(self.screen, (55, 65, 80), pygame.Rect(rx, ry, robot_w, robot_h), 2) + + # Pozycja montażowa fizyczna dwóch wewnętrznych LiDARów (X=-2.0, Y= +/- 1.5) + pygame.draw.circle(self.screen, (0, 255, 0), (cx - int(1.5*scale), cy + int(2.0*scale)), 5) # Lewy + pygame.draw.circle(self.screen, (255, 150, 0), (cx - int(-1.5*scale), cy + int(2.0*scale)), 5) # Prawy + + font = pygame.font.SysFont('Monospace', 13, bold=True) + if self.current_state == self.STATE_INITIALIZING: + status_text = "INITIALIZING: Oczekiwanie na" + if not self.has_odom: status_text += " [ODOM]" + if self.latest_left_scan is None: status_text += " [LIDAR_L]" + if self.latest_right_scan is None: status_text += " [LIDAR_R]" + self.screen.blit(font.render(status_text, True, (255, 140, 0)), (15, 15)) + pygame.display.flip() + return + + # Rysowanie przeliczonych chmur punktów (Lewa ściana = zielona, Prawa = pomarańczowa) + for x, y in zip(self.pts_L[0], self.pts_L[1]): + px = cx - int(y * scale) + py = cy - int(x * scale) + if 0 <= px < self.win_size and 0 <= py < self.win_size: + pygame.draw.circle(self.screen, (100, 255, 100), (px, py), 2) + + for x, y in zip(self.pts_R[0], self.pts_R[1]): + px = cx - int(y * scale) + py = cy - int(x * scale) + if 0 <= px < self.win_size and 0 <= py < self.win_size: + pygame.draw.circle(self.screen, (255, 165, 0), (px, py), 2) + + # Wyświetlanie tekstów diagnostycznych + self.screen.blit(font.render(f"STAN: {self.current_state}", True, (255, 255, 255)), (15, 15)) + self.screen.blit(font.render(f"DYSTANS OD LEWEJ ŚCIANY: {self.avg_left:.2f}m", True, (100, 255, 100)), (15, 35)) + self.screen.blit(font.render(f"DYSTANS OD PRAWEJ ŚCIANY: {self.avg_right:.2f}m", True, (255, 165, 0)), (15, 55)) + pygame.display.flip() + +def main(args=None): + rclpy.init(args=args) + node = HarvesterStateMachine() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.stop_robot() + node.destroy_node() + rclpy.shutdown() + pygame.quit() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/world_description/models/Mud Box/materials/scripts/mud.material b/src/world_description/models/Mud Box/materials/scripts/mud.material new file mode 100644 index 0000000..bc32164 --- /dev/null +++ b/src/world_description/models/Mud Box/materials/scripts/mud.material @@ -0,0 +1,19 @@ +material vrc/mud +{ + technique + { + pass + { + ambient 0.5 0.5 0.5 1.0 + diffuse 0.5 0.5 0.5 1.0 + specular 0.2 0.2 0.2 1.0 12.5 + + texture_unit + { + texture mud_soft_leaves.png + filtering anistropic + max_anisotropy 16 + } + } + } +} diff --git a/src/world_description/models/Mud Box/model-1_4.sdf b/src/world_description/models/Mud Box/model-1_4.sdf new file mode 100644 index 0000000..f43af41 --- /dev/null +++ b/src/world_description/models/Mud Box/model-1_4.sdf @@ -0,0 +1,80 @@ + + + + true + + + + + 8 10 0.2 + + + + + -2 2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + 2 2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + 2 -2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + -2 -2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + + + diff --git a/src/world_description/models/Mud Box/model.config b/src/world_description/models/Mud Box/model.config new file mode 100644 index 0000000..c2bdb91 --- /dev/null +++ b/src/world_description/models/Mud Box/model.config @@ -0,0 +1,18 @@ + + + + Mud Box + 1.0 + model-1_4.sdf + model.sdf + + + Thomas Koletschka + thomas.koletschka@gmail.com + + + + A mud textured plane. + + + diff --git a/src/world_description/models/Mud Box/model.sdf b/src/world_description/models/Mud Box/model.sdf new file mode 100644 index 0000000..0319d01 --- /dev/null +++ b/src/world_description/models/Mud Box/model.sdf @@ -0,0 +1,79 @@ + + + + true + + + + + 8 10 0.2 + + + + + -2 2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + 2 2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + 2 -2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + -2 -2.5 0 0 0 0 + false + + + 4 5 0.2 + + + + + + + + + diff --git a/src/world_description/models/Mud Box/thumbnails/1.png b/src/world_description/models/Mud Box/thumbnails/1.png new file mode 100644 index 0000000..9dd6d36 Binary files /dev/null and b/src/world_description/models/Mud Box/thumbnails/1.png differ diff --git a/src/world_description/models/Mud Box/thumbnails/2.png b/src/world_description/models/Mud Box/thumbnails/2.png new file mode 100644 index 0000000..beb6ced Binary files /dev/null and b/src/world_description/models/Mud Box/thumbnails/2.png differ diff --git a/src/world_description/models/Mud Box/thumbnails/3.png b/src/world_description/models/Mud Box/thumbnails/3.png new file mode 100644 index 0000000..89e1d7a Binary files /dev/null and b/src/world_description/models/Mud Box/thumbnails/3.png differ diff --git a/src/world_description/models/Mud Box/thumbnails/4.png b/src/world_description/models/Mud Box/thumbnails/4.png new file mode 100644 index 0000000..68e6056 Binary files /dev/null and b/src/world_description/models/Mud Box/thumbnails/4.png differ diff --git a/src/world_description/models/Mud Box/thumbnails/5.png b/src/world_description/models/Mud Box/thumbnails/5.png new file mode 100644 index 0000000..ccb740d Binary files /dev/null and b/src/world_description/models/Mud Box/thumbnails/5.png differ diff --git a/src/world_description/models/corn.sdf b/src/world_description/models/corn.sdf new file mode 100644 index 0000000..d586de2 --- /dev/null +++ b/src/world_description/models/corn.sdf @@ -0,0 +1,24 @@ + + + + true + + + + + meshes/scene.gltf + + + + + + + 0 0 0.5 0 0 0 + + 0.15 1.0 + + + + + + \ No newline at end of file diff --git a/src/world_description/models/meshes/license.txt b/src/world_description/models/meshes/license.txt new file mode 100644 index 0000000..35e5074 --- /dev/null +++ b/src/world_description/models/meshes/license.txt @@ -0,0 +1,11 @@ +Model Information: +* title: Corn! Corn! Corn! +* source: https://sketchfab.com/3d-models/corn-corn-corn-10187bc37c9e42ef8770b28452ee7cd3 +* author: Tiia Tuulia (https://sketchfab.com/tiiatuulia) + +Model License: +* license type: CC-BY-4.0 (http://creativecommons.org/licenses/by/4.0/) +* requirements: Author must be credited. Commercial use is allowed. + +If you use this 3D model in your project be sure to copy paste this credit wherever you share it: +This work is based on "Corn! Corn! Corn!" (https://sketchfab.com/3d-models/corn-corn-corn-10187bc37c9e42ef8770b28452ee7cd3) by Tiia Tuulia (https://sketchfab.com/tiiatuulia) licensed under CC-BY-4.0 (http://creativecommons.org/licenses/by/4.0/) \ No newline at end of file diff --git a/src/world_description/models/meshes/mud_soft_leaves.png b/src/world_description/models/meshes/mud_soft_leaves.png new file mode 100644 index 0000000..9ca7198 Binary files /dev/null and b/src/world_description/models/meshes/mud_soft_leaves.png differ diff --git a/src/world_description/models/meshes/scene.bin b/src/world_description/models/meshes/scene.bin new file mode 100644 index 0000000..2d71758 Binary files /dev/null and b/src/world_description/models/meshes/scene.bin differ diff --git a/src/world_description/models/meshes/scene.gltf b/src/world_description/models/meshes/scene.gltf new file mode 100644 index 0000000..44d91a6 --- /dev/null +++ b/src/world_description/models/meshes/scene.gltf @@ -0,0 +1,394 @@ +{ + "accessors": [ + { + "bufferView": 2, + "componentType": 5126, + "count": 2029, + "max": [ + 10.655570983886719, + 11.136487007141113, + 4.473674774169922 + ], + "min": [ + -11.730097770690918, + -8.04345703125, + -41.532630920410156 + ], + "type": "VEC3" + }, + { + "bufferView": 2, + "byteOffset": 24348, + "componentType": 5126, + "count": 2029, + "max": [ + 0.9976066946983337, + 1.0, + 0.9962291717529297 + ], + "min": [ + -0.9968279004096985, + -0.994667649269104, + -0.990868330001831 + ], + "type": "VEC3" + }, + { + "bufferView": 1, + "componentType": 5126, + "count": 2029, + "max": [ + 0.5003870129585266, + 0.9281949996948242 + ], + "min": [ + 0.062497999519109726, + 0.058719001710414886 + ], + "type": "VEC2" + }, + { + "bufferView": 0, + "componentType": 5125, + "count": 7188, + "type": "SCALAR" + }, + { + "bufferView": 2, + "byteOffset": 48696, + "componentType": 5126, + "count": 146, + "max": [ + 3.683738946914673, + 0.2560949921607971, + -1.8365540504455566 + ], + "min": [ + -1.6435259580612183, + -11.871248245239258, + -31.080829620361328 + ], + "type": "VEC3" + }, + { + "bufferView": 2, + "byteOffset": 50448, + "componentType": 5126, + "count": 146, + "max": [ + 0.9527933597564697, + 0.8330304622650146, + 0.9927104711532593 + ], + "min": [ + -0.9898576140403748, + -0.9799456596374512, + -0.9875993728637695 + ], + "type": "VEC3" + }, + { + "bufferView": 1, + "byteOffset": 16232, + "componentType": 5126, + "count": 146, + "max": [ + 0.1911740005016327, + 0.9113199710845947 + ], + "min": [ + 0.062497999519109726, + 0.058719001710414886 + ], + "type": "VEC2" + }, + { + "bufferView": 0, + "byteOffset": 28752, + "componentType": 5125, + "count": 522, + "type": "SCALAR" + }, + { + "bufferView": 3, + "componentType": 5126, + "count": 1, + "max": [ + 0.0416666679084301 + ], + "min": [ + 0.0416666679084301 + ], + "type": "SCALAR" + }, + { + "bufferView": 4, + "componentType": 5126, + "count": 1, + "max": [ + -0.27267909049987793, + 0.6607204675674438, + 0.6418024897575378, + 0.2778203785419464 + ], + "min": [ + -0.27267909049987793, + 0.6607204675674438, + 0.6418024897575378, + 0.2778203785419464 + ], + "type": "VEC4" + } + ], + "animations": [ + { + "channels": [ + { + "sampler": 0, + "target": { + "node": 4, + "path": "rotation" + } + } + ], + "name": "Default Take", + "samplers": [ + { + "input": 8, + "interpolation": "LINEAR", + "output": 9 + } + ] + } + ], + "asset": { + "extras": { + "author": "Tiia Tuulia (https://sketchfab.com/tiiatuulia)", + "license": "CC-BY-4.0 (http://creativecommons.org/licenses/by/4.0/)", + "source": "https://sketchfab.com/3d-models/corn-corn-corn-10187bc37c9e42ef8770b28452ee7cd3", + "title": "Corn! Corn! Corn!" + }, + "generator": "Sketchfab-12.68.0", + "version": "2.0" + }, + "bufferViews": [ + { + "buffer": 0, + "byteLength": 30840, + "name": "floatBufferViews", + "target": 34963 + }, + { + "buffer": 0, + "byteLength": 17400, + "byteOffset": 30840, + "byteStride": 8, + "name": "floatBufferViews", + "target": 34962 + }, + { + "buffer": 0, + "byteLength": 52200, + "byteOffset": 48240, + "byteStride": 12, + "name": "floatBufferViews", + "target": 34962 + }, + { + "buffer": 0, + "byteLength": 4, + "byteOffset": 100440, + "name": "floatBufferViews" + }, + { + "buffer": 0, + "byteLength": 16, + "byteOffset": 100444, + "byteStride": 16, + "name": "floatBufferViews" + } + ], + "buffers": [ + { + "byteLength": 100460, + "uri": "scene.bin" + } + ], + "images": [ + { + "uri": "textures/corn_plant__corn_texture_png_baseColor.png" + } + ], + "materials": [ + { + "doubleSided": true, + "name": "corn_plant__corn_texture_png", + "pbrMetallicRoughness": { + "baseColorTexture": { + "index": 0 + }, + "metallicFactor": 0.0, + "roughnessFactor": 0.6541539634146342 + } + }, + { + "doubleSided": true, + "name": "corn_plant_001__corn_texture_png_001", + "pbrMetallicRoughness": { + "baseColorTexture": { + "index": 0 + }, + "metallicFactor": 0.0, + "roughnessFactor": 0.6 + } + } + ], + "meshes": [ + { + "name": "Circle_020_corn_plant__corn_texture_png_0", + "primitives": [ + { + "attributes": { + "NORMAL": 1, + "POSITION": 0, + "TEXCOORD_0": 2 + }, + "indices": 3, + "material": 0, + "mode": 4 + } + ] + }, + { + "name": "Circle_020_corn_plant_001__corn_texture_png_001_0", + "primitives": [ + { + "attributes": { + "NORMAL": 5, + "POSITION": 4, + "TEXCOORD_0": 6 + }, + "indices": 7, + "material": 1, + "mode": 4 + } + ] + } + ], + "nodes": [ + { + "children": [ + 1 + ], + "matrix": [ + 0.6242683529853821, + 0.0, + 0.0, + 0.0, + 0.0, + 1.3861541980583915e-16, + -0.6242683529853821, + 0.0, + 0.0, + 0.6242683529853821, + 1.3861541980583915e-16, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + "name": "Sketchfab_model" + }, + { + "children": [ + 2 + ], + "matrix": [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + -1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + "name": "fdd38405c86b44e49e5f7a62b0d2f0f7.fbx" + }, + { + "children": [ + 3 + ], + "name": "Object_2" + }, + { + "children": [ + 4 + ], + "name": "RootNode" + }, + { + "children": [ + 5, + 6 + ], + "name": "Circle_020", + "rotation": [ + -0.27267906069755554, + 0.6607205271720886, + 0.6418024301528931, + 0.277820348739624 + ], + "scale": [ + 0.08378752321004868, + 0.08378753066062927, + 0.08378753066062927 + ], + "translation": [ + 0.03753768280148506, + 3.4483883380889893, + 0.0886821448802948 + ] + }, + { + "mesh": 0, + "name": "Circle_020_corn_plant__corn_texture_png_0" + }, + { + "mesh": 1, + "name": "Circle_020_corn_plant_001__corn_texture_png_001_0" + } + ], + "samplers": [ + { + "magFilter": 9729, + "minFilter": 9987, + "wrapS": 10497, + "wrapT": 10497 + } + ], + "scene": 0, + "scenes": [ + { + "name": "Sketchfab_Scene", + "nodes": [ + 0 + ] + } + ], + "textures": [ + { + "sampler": 0, + "source": 0 + } + ] +} diff --git a/src/world_description/models/meshes/textures/corn_plant__corn_texture_png_baseColor.png b/src/world_description/models/meshes/textures/corn_plant__corn_texture_png_baseColor.png new file mode 100644 index 0000000..8bf4e14 Binary files /dev/null and b/src/world_description/models/meshes/textures/corn_plant__corn_texture_png_baseColor.png differ diff --git a/src/world_description/models/model.config b/src/world_description/models/model.config new file mode 100644 index 0000000..32d4c27 --- /dev/null +++ b/src/world_description/models/model.config @@ -0,0 +1,12 @@ + + + + Corn + 1.0 + corn.sdf + + + Corn plant + + + \ No newline at end of file