diff --git a/src/arm_harverster/scripts/arm_point_controller.py b/src/arm_harverster/scripts/arm_point_controller.py index c40f751..abea795 100755 --- a/src/arm_harverster/scripts/arm_point_controller.py +++ b/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() - position_constraint.header.frame_id = "base_link" - position_constraint.link_name = "tool_link" + # Add target pose to the path request constraints + goal_constraints = Constraints() + 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 - position_constraint.weight = 1.0 - - orient_constraint = OrientationConstraint() - orient_constraint.header.frame_id = "base_link" - orient_constraint.link_name = "tool_link" - orient_constraint.orientation = target_pose.pose.orientation + # Orientation Constraint for the destination frame + ori_con = OrientationConstraint() + ori_con.header.frame_id = "base_link" + ori_con.link_name = "tool_link" + ori_con.orientation = target_pose.pose.orientation + ori_con.absolute_x_axis_tolerance = 0.05 + 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 - 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) + goal_constraints.position_constraints.append(pos_con) + goal_constraints.orientation_constraints.append(ori_con) 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): - """ - 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 + """Fetches live position from the simulation and advances the camera straight forward.""" 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 - success = node.send_pt_to_pt_goal(-0.4, -0.3, 0.5) + # Coordinates are now evaluated relative to the arm's base link. + # 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) - - # 2. Call the sweep without passing any XYZ parameters! - # It handles the math completely internally. - #node.execute_straight_camera_sweep(distance=0.2) + # Execute the forward crawl sweep + node.execute_straight_camera_sweep(distance=0.2) node.destroy_node() rclpy.shutdown() diff --git a/src/arm_moveit_config/launch/sim.launch.py b/src/arm_moveit_config/launch/sim.launch.py index 9c90d2e..f340e66 100644 --- a/src/arm_moveit_config/launch/sim.launch.py +++ b/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', diff --git a/src/robotarm_description/urdf/robot_arm.urdf b/src/robotarm_description/urdf/robot_arm.urdf index b0b3f3d..e5cb8b0 100644 --- a/src/robotarm_description/urdf/robot_arm.urdf +++ b/src/robotarm_description/urdf/robot_arm.urdf @@ -13,13 +13,6 @@ - @@ -214,6 +207,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 -1.5708 0 + 1 + 30 + true + arm_camera/image_raw + + 1.2 + + 640 + 480 + R8G8B8 + + + 0.05 + 10.0 + + + + gz_ros2_control/GazeboSimSystem diff --git a/src/robotarm_description/urdf/robot_arm.urdf.xacro b/src/robotarm_description/urdf/robot_arm.urdf.xacro index c961ae3..48e9e61 100644 --- a/src/robotarm_description/urdf/robot_arm.urdf.xacro +++ b/src/robotarm_description/urdf/robot_arm.urdf.xacro @@ -20,14 +20,6 @@ - - @@ -194,6 +186,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 -1.5708 0 + 1 + 30 + true + arm_camera/image_raw + 1.2 + 640 + 480 + R8G8B8 + + + 0.05 + 10.0 + + + + + gz_ros2_control/GazeboSimSystem