Publishing to a ROS topic to open robot hand through Python script

Question

I am a trying to publish to a node through a Python script. The message to be published is supposed to open or close the hand of a robotic arm (Franka Research 3), which can be easily done using rostopic pub on the command line.

To open: rostopic pub --once /franka_gripper/move/goal franka_gripper/MoveActionGoal "goal: { width: 0.08, speed: 0.1 }"

To close: rostopic pub --once /franka_gripper/grasp/goal franka_gripper/GraspActionGoal "goal: { width: 0.03, epsilon:{ inner: 0.005, outer: 0.005 }, speed: 0.1, force: 5.0}"

This works with no problem, but I am trying to do this through a Python script to make my life easier and not have to run both commands all the time. I am trying to use the following lines of code to open the hand:

open_pub = rospy.Publisher('/franka_gripper/move/goal', franka_gripper.msg.MoveActionGoal, queue_size=10)
open_pub.publish(goal=(0.08, 0.1))

Running the script returns no errors. Other parts of the script used to move the other joints of the arm work, but the hand doesn't move. I'm not sure if I may be missing something and I would really appreciate some help. Thanks.

Edit: Here is the whole script:

from __future__ import print_function
from six.moves import input

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

try:
    from math import pi, tau, dist, fabs, cos
except:  # For Python 2 compatibility
    from math import pi, fabs, cos, sqrt

    tau = 2.0 * pi

    def dist(p, q):
        return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q)))


from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import franka_msgs.msg
import franka_gripper.msg

def all_close(goal, actual, tolerance):
    if type(goal) is list:
        for index in range(len(goal)):
            if abs(actual[index] - goal[index]) > tolerance:
                return False

    elif type(goal) is geometry_msgs.msg.PoseStamped:
        return all_close(goal.pose, actual.pose, tolerance)

    elif type(goal) is geometry_msgs.msg.Pose:
        x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual)
        x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal)
        # Euclidean distance
        d = dist((x1, y1, z1), (x0, y0, z0))
        # phi = angle between orientations
        cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)
        return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0)

    return True


class Trajectory(object):
    def __init__(self):
        super(Trajectory, self).__init__()
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node("trajectory", anonymous=True)

        robot = moveit_commander.RobotCommander()

        scene = moveit_commander.PlanningSceneInterface()

        group_name = "fre_arm"
        move_group = moveit_commander.MoveGroupCommander(group_name)

        display_trajectory_publisher = rospy.Publisher(
            "/move_group/display_planned_path",
            moveit_msgs.msg.DisplayTrajectory,
            queue_size=20,
        )

        planning_frame = move_group.get_planning_frame()
        eef_link = move_group.get_end_effector_link()
        group_names = robot.get_group_names()

        self.box_name = ""
        self.robot = robot
        self.scene = scene
        self.move_group = move_group
        self.display_trajectory_publisher = display_trajectory_publisher
        self.planning_frame = planning_frame
        self.eef_link = eef_link
        self.group_names = group_names
        
    def go_to_joint_state(self, joint1, joint2, joint3, joint4, joint5, joint6, joint7):
        move_group = self.move_group
        
        joint_goal = move_group.get_current_joint_values()
        joint_goal[0] = joint1
        joint_goal[1] = joint2
        joint_goal[2] = joint3
        joint_goal[3] = joint4
        joint_goal[4] = joint5
        joint_goal[5] = joint6
        joint_goal[6] = joint7

        move_group.go(joint_goal, wait=True)

        move_group.stop()

        current_joints = move_group.get_current_joint_values()
        print(all_close(joint_goal, current_joints, 0.01))
        return all_close(joint_goal, current_joints, 0.01)

def main():
    try:
        traj = Trajectory()
        
        # Open hand
        open_pub = rospy.Publisher('/franka_gripper/move/goal', franka_gripper.msg.MoveActionGoal, queue_size=10)
        open_pub.publish(goal=(0.08, 0.1))
        
        # Move arm
        traj.go_to_joint_state(tau/18, -tau/5.71428571, -tau/6.54545455, -tau/3.75, -tau/6.42857143, tau/5.71428571, -tau/51.4285714)

    except rospy.ROSInterruptException:
        return
    except KeyboardInterrupt:
        return


if __name__ == "__main__":
    main()

Answer 1

It looks like(at least part) of your issue is that you're trying to publish a message and then the node immediately tries to exit. You need something to block the main thread for some amount of time. In this case it could be something as simple as adding a rospy.spin() after your call to traj.go_to_joint_state

Note that this will block your node until it receives a signal to stop.