如何用Python脚本自动化ROS终端命令？能否复用已有订阅器？

Hey there! Let's walk through how to solve your problem—reusing your existing subscriber logic and building an automated script to monitor and move your dVRK PSM1 arm.

1. Definitely Reuse Your Existing Subscriber Logic #

Your arm.py already has a well-implemented subscriber and callback that handles converting ROS PoseStamped messages to posemath objects. Reusing this is the best approach because:

• It avoids redundant code (no need to redefine the subscriber or message conversion)
• It maintains consistent state management for the arm's current position
• You don't have to re-implement error handling or message parsing that's already in place

To make this usable in your new script, add a public getter method to your Arm class in arm.py (since the __position_cartesian_current attribute is private):

# Add this to your Arm class in arm.py
def get_current_cartesian_position(self):
    """Return the current Cartesian position as a posemath object"""
    return self.__position_cartesian_current

2. Build the Automated Script #

Here's a complete script that reuses your Arm class, monitors the current position (replacing rostopic echo), and moves the arm to a target position:

import rospy
from arm import Arm  # Import your Arm class from arm.py
from geometry_msgs.msg import PoseStamped
from tf_conversions import posemath

def move_to_target(arm, target_pose):
    """Publish a target Cartesian pose and wait for the arm to reach it"""
    # Create publisher for target pose topic (adjust the namespace suffix as needed)
    target_pub = rospy.Publisher(
        arm._Arm__full_ros_namespace + '/position_cartesian_target',
        PoseStamped,
        queue_size=10
    )
    # Wait for publisher to connect to subscribers
    rospy.sleep(0.5)

    # Build the PoseStamped message
    target_msg = PoseStamped()
    target_msg.header.stamp = rospy.Time.now()
    target_msg.header.frame_id = 'world'  # Match your robot's base frame
    target_msg.pose = posemath.toMsg(target_pose)

    # Send the target command
    target_pub.publish(target_msg)
    rospy.loginfo("Sent target Cartesian pose to the arm")

    # Wait for the arm to reach the target (adjust tolerance for your needs)
    position_tolerance = 0.005  # 5mm tolerance
    while not rospy.is_shutdown():
        current_pose = arm.get_current_cartesian_position()
        # Calculate position differences
        x_diff = abs(current_pose.pose.position.x - target_pose.pose.position.x)
        y_diff = abs(current_pose.pose.position.y - target_pose.pose.position.y)
        z_diff = abs(current_pose.pose.position.z - target_pose.pose.position.z)

        if x_diff < position_tolerance and y_diff < position_tolerance and z_diff < position_tolerance:
            rospy.loginfo("Arm has reached the target position!")
            break
        # Print current position (replaces rostopic echo)
        rospy.loginfo(f"Current Position: X={current_pose.pose.position.x:.4f}, Y={current_pose.pose.position.y:.4f}, Z={current_pose.pose.position.z:.4f}")
        rospy.sleep(0.1)

if __name__ == '__main__':
    try:
        # Initialize ROS node
        rospy.init_node('dvrk_psm1_auto_controller', anonymous=True)

        # Instantiate your Arm class (replace with your actual namespace)
        psm1_arm = Arm(full_ros_namespace='dvrk/PSM1')

        # Wait for the subscriber to receive the first position message
        rospy.sleep(1.0)

        # Get and log the initial current position
        initial_position = psm1_arm.get_current_cartesian_position()
        rospy.loginfo(f"Initial Cartesian Position: {initial_position}")

        # Define your target pose (example: move down 5cm from initial position)
        target_pose = posemath.fromPose(initial_position.pose)
        target_pose.pose.position.z -= 0.05  # Adjust this to your desired target

        # Execute the move
        move_to_target(psm1_arm, target_pose)

    except rospy.ROSInterruptException:
        rospy.loginfo("Script interrupted")

3. Key Notes to Adjust for Your Setup #

• Namespace: Make sure full_ros_namespace matches your robot's actual ROS namespace (e.g., dvrk/PSM1)
• Coordinate Frame: The frame_id in the target message should match the frame your arm uses (common ones are world or base_link)
• Tolerance: Adjust position_tolerance based on how precise your movement needs to be
• Target Pose: Replace the example target with your desired Cartesian position (you can also set custom quaternions for orientation)
• Arm Class Initialization: If your Arm class requires additional parameters (like robot type, etc.), adjust the instantiation line to match

内容的提问来源于stack exchange，提问作者compscistudent