kamera na ramiemniu

src/arm_harverster/scripts/arm_point_controller.py

@@ -7,9 +7,8 @@ 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.msg import Constraints, 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
 
@@ -32,7 +31,7 @@ class CameraArmController(Node):
         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)
+        # 4. 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()
@@ -64,33 +63,30 @@ class CameraArmController(Node):
 
     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 = "base_link"
-        req.fk_link_names = ["tool_link"] # Name of the tip frame to track
+        req.fk_link_names = ["tool_link"] 
         
-        # 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
+        if res and res.error_code.val == 1: 
             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"""
+        """Moves the camera to a specific 3D coordinate freely, settling horizontally at the end destination"""
         self.get_logger().info(f"Planning Point-to-Point Move to: X={x}, Y={y}, Z={z}")
         
         goal_msg = MoveGroup.Goal()
@@ -99,21 +95,37 @@ class CameraArmController(Node):
         goal_msg.request.num_planning_attempts = 15
         goal_msg.request.pipeline_id = "ompl"
         
+        # Define the target position and orientation
         target_pose = PoseStamped()
         target_pose.header.frame_id = "base_link"
         target_pose.pose.position.x = float(x)
         target_pose.pose.position.y = float(y)
         target_pose.pose.position.z = float(z)
+        
+        # Enforces the horizontal alignment strictly at the final destination pose
         target_pose.pose.orientation = self.get_horizontal_camera_quaternion()
 
-        position_constraint = PositionConstraint()
+        # Add target pose to the path request constraints
-        position_constraint.header.frame_id = "base_link"
+        goal_constraints = Constraints()
-        position_constraint.link_name = "tool_link"
+        goal_msg.request.goal_constraints.append(goal_constraints)
         
+        # MoveIt handles position and end-point orientation without path constraints
+        # By avoiding constraints during movement, the planner should avoid RRTConnect failures.
+        goal_msg.request.workspace_parameters.header.frame_id = "base_link"
+        
+        # Pack the target pose directly into the goal message requests
+        from moveit_msgs.msg import PositionConstraint, OrientationConstraint
+        
+        # Create a simple end-state constraint matching our target pose
+        # Position Constraint
+        pos_con = PositionConstraint()
+        pos_con.header.frame_id = "base_link"
+        pos_con.link_name = "tool_link"
+        from moveit_msgs.msg import BoundingVolume
+        from shape_msgs.msg import SolidPrimitive
         box = SolidPrimitive()
         box.type = SolidPrimitive.BOX
-        box.dimensions = [0.03, 0.03, 0.03]
+        box.dimensions = [0.01, 0.01, 0.01] # Tight convergence tolerance
-        
         volume = BoundingVolume()
         volume.primitives.append(box)
         
@@ -121,24 +133,21 @@ class CameraArmController(Node):
         box_pose.position = target_pose.pose.position
         box_pose.orientation.w = 1.0
         volume.primitive_poses.append(box_pose)
+        pos_con.constraint_region = volume
+        pos_con.weight = 1.0
         
-        position_constraint.constraint_region = volume
+        # Orientation Constraint for the destination frame
-        position_constraint.weight = 1.0
+        ori_con = OrientationConstraint()
-
+        ori_con.header.frame_id = "base_link"
-        orient_constraint = OrientationConstraint()
+        ori_con.link_name = "tool_link"
-        orient_constraint.header.frame_id = "base_link"
+        ori_con.orientation = target_pose.pose.orientation
-        orient_constraint.link_name = "tool_link"
+        ori_con.absolute_x_axis_tolerance = 0.05
-        orient_constraint.orientation = target_pose.pose.orientation
+        ori_con.absolute_y_axis_tolerance = 0.05
+        ori_con.absolute_z_axis_tolerance = 0.05
+        ori_con.weight = 1.0
         
-        orient_constraint.absolute_x_axis_tolerance = 0.15  
+        goal_constraints.position_constraints.append(pos_con)
-        orient_constraint.absolute_y_axis_tolerance = 0.15  
+        goal_constraints.orientation_constraints.append(ori_con)
-        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)
@@ -154,11 +163,7 @@ class CameraArmController(Node):
         return True
 
     def execute_straight_camera_sweep(self, distance=0.2):
-        """
+        """Fetches live position from the simulation and advances the camera straight forward."""
-        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.")
@@ -169,14 +174,12 @@ class CameraArmController(Node):
         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 = "base_link"
         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
@@ -187,7 +190,6 @@ class CameraArmController(Node):
         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)
         
@@ -218,15 +220,15 @@ def main(args=None):
     rclpy.init(args=args)
     node = CameraArmController()
 
-    # 1. Send to starting exploration zone
+    # Coordinates are now evaluated relative to the arm's base link.
-    success = node.send_pt_to_pt_goal(-0.4, -0.3, 0.5)
+    # Reaches 40cm forward, 30cm down toward crops. It will rotate freely during
+    # transit and level out perfectly flat once it locks into the target destination.
+    success = node.send_pt_to_pt_goal(.2, 0.3, 0.3)
     
     if success:
         time.sleep(2.0) 
-        
+        # Execute the forward crawl sweep
-        # 2. Call the sweep without passing any XYZ parameters! 
+        node.execute_straight_camera_sweep(distance=0.2)
-        # It handles the math completely internally.
-        #node.execute_straight_camera_sweep(distance=0.2)
 
     node.destroy_node()
     rclpy.shutdown()

src/arm_moveit_config/launch/sim.launch.py

@@ -63,6 +63,7 @@ def generate_launch_description():
         arguments=[
             '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
             '/world/corn_field_wide/model/harvester_robot/joint_state@sensor_msgs/msg/JointState[gz.msgs.Model',
+            '/arm_camera/image_raw@sensor_msgs/msg/Image[gz.msgs.Image',
             # '/model/harvester_robot/joint/joint_1/cmd_pos@std_msgs/msg/Float64]gz.msgs.Double',
             # 'cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist',
             # 'model/harvester_robot/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry',

src/robotarm_description/urdf/robot_arm.urdf

@@ -13,13 +13,6 @@
   <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"/>
@@ -214,6 +207,54 @@
     <child link="tool_link"/>
     <origin rpy="0 0 0" xyz="0 0 0.064"/>
   </joint>
+  <link name="arm_camera_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.03"/>
+      </geometry>
+      <material name="dark_grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.03"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="tool_to_camera_joint" type="fixed">
+    <parent link="tool_link"/>
+    <child link="arm_camera_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.03"/>
+  </joint>
+  <gazebo reference="arm_camera_link">
+    <sensor name="arm_camera" type="camera">
+      <pose>0 0 0 0 -1.5708 0</pose>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>true</visualize>
+      <topic>arm_camera/image_raw</topic>
+      <camera>
+        <horizontal_fov>1.2</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.05</near>
+          <far>10.0</far>
+        </clip>
+      </camera>
+    </sensor>
+  </gazebo>
   <ros2_control name="GazeboSimSystem" type="system">
     <hardware>
       <plugin>gz_ros2_control/GazeboSimSystem</plugin>

src/robotarm_description/urdf/robot_arm.urdf.xacro

@@ -20,14 +20,6 @@
     </inertial>
   </xacro:macro>
 
-  <!-- <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 xyz="0 0 0.05" rpy="0 0 0"/>
@@ -194,6 +186,54 @@
     <origin xyz="0 0 ${0.04 * LENGTH_SCALE}" rpy="0 0 0"/>
   </joint>
 
+  <link name="arm_camera_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.03"/> </geometry>
+      <material name="dark_grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.03"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="tool_to_camera_joint" type="fixed">
+    <parent link="tool_link"/>
+    <child link="arm_camera_link"/>
+    <origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
+  </joint>
+
+  <gazebo reference="arm_camera_link">
+    <sensor name="arm_camera" type="camera">
+      <pose>0 0 0 0 -1.5708 0</pose>
+      <always_on>1</always_on>
+      <update_rate>30</update_rate>
+      <visualize>true</visualize>
+      <topic>arm_camera/image_raw</topic> <camera>
+        <horizontal_fov>1.2</horizontal_fov> <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.05</near>
+          <far>10.0</far>
+        </clip>
+      </camera>
+    </sensor>
+  </gazebo>
+
   <ros2_control name="GazeboSimSystem" type="system">
     <hardware>
       <plugin>gz_ros2_control/GazeboSimSystem</plugin>