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

kamera na ramiemniu

Browse Source
This commit is contained in:
Marcin M
2026-06-10 23:16:47 +02:00
parent 65c9e81858
commit f407eae6bc
4 changed files with 143 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_harverster/scripts/arm_point_controller.py
+46 -44
View File
@@ -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
# 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 = OrientationConstraint()
orient_constraint.header.frame_id = "base_link"
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)
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()
src/arm_moveit_config/launch/sim.launch.py
+1
View File
@@ -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
+48 -7
View File
@@ -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
+48 -8
View File
@@ -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>