-1
$\begingroup$

I am using darknet_ros_3d package and getting the published 3D coordinates. Now I need to subscribe these coordinates into a Python file (main.py) of another package named kth-ros-pkg/yumi_demos. The code of main.py is given as below:

import sys
import copy
import rospy
import moveit_commander
import yumi_moveit_utils as yumi
import moveit_msgs.msg
import geometry_msgs.msg
from std_srvs.srv import Empty




def close_grippers(arm):
    """Closes the grippers.
    Closes the grippers with an effort of 15 and then relaxes the effort to 0.
    :param arm: The side to be closed (moveit_utils LEFT or RIGHT)
    :type arm: int
    :returns: Nothing
    :rtype: None
    """
    yumi.gripper_effort(arm, 15.0)
    yumi.gripper_effort(arm, 0.0)

def open_grippers(arm):
    """Opens the grippers.
    Opens the grippers with an effort of -15 and then relaxes the effort to 0.
    :param arm: The side to be opened (moveit_utils LEFT or RIGHT)
    :type arm: int
    :returns: Nothing
    :rtype: None
    """
    yumi.gripper_effort(arm, -15.0)
    yumi.gripper_effort(arm, 0.0)



def move_and_grasp(arm, pose_ee, grip_effort):
    try:
        yumi.traverse_path([pose_ee], arm, 10)
    except Exception:
        if (arm == yumi.LEFT):
            yumi.plan_and_move(yumi.group_l, yumi.create_pose_euler(pose_ee[0], pose_ee[1], pose_ee[2], pose_ee[3], pose_ee[4], pose_ee[5]))
        elif (arm == yumi.RIGHT):
            yumi.plan_and_move(yumi.group_r, yumi.create_pose_euler(pose_ee[0], pose_ee[1], pose_ee[2], pose_ee[3], pose_ee[4], pose_ee[5]))

    if (grip_effort <= 20 and grip_effort >= -20):
        yumi.gripper_effort(arm, grip_effort)
    else:
        print("The gripper effort values should be in the range [-20, 20]")




def run():
    """Starts the node
    Runs to start the node and initialize everthing. Runs forever via Spin()
    :returns: Nothing
    :rtype: None
    """

    rospy.init_node('yumi_moveit_demo')

    #Start by connecting to ROS and MoveIt!
    yumi.init_Moveit()


    # Print current joint angles
    yumi.print_current_joint_states(yumi.RIGHT)
    yumi.print_current_joint_states(yumi.LEFT)

    # Reset YuMi joints to "home" position
    yumi.reset_pose()


    # Drive YuMi end effectors to a desired position (pose_ee), and perform a grasping task with a given effort (grip_effort)
    # Gripper effort: opening if negative, closing if positive, static if zero
    pose_ee = [0.3, 0.15, 0.2, 0.0, 3.14, 3.14]
    grip_effort = -10.0
    move_and_grasp(yumi.LEFT, pose_ee, grip_effort)

    pose_ee = [0.3, -0.15, 0.2, 0.0, 3.14, 3.14]
    grip_effort = -10.0
    move_and_grasp(yumi.RIGHT, pose_ee, grip_effort)

    rospy.spin()




if __name__ == '__main__':
    try:
        run()

        print "####################################     Program finished     ####################################"
    except rospy.ROSInterruptException:
        pass 

And I am getting coordinates from the darknet package in the following format:

header:
  stamp:
    sec: 1593723845
    nanosec: 430724839
  frame_id: camera_link
bounding_boxes:
- object_name: person
  probability: 0.7609682679176331
  xmin: 0.4506256580352783
  ymin: -0.3164764642715454
  xmax: 0.7936256527900696
  ymax: 0.11368180811405182
  zmin: -0.25958430767059326
  zmax: 0.10506562888622284
$\endgroup$
0
$\begingroup$

Subscribing to messages from a different package is no different than from the same package, just make sure to get the topic name right in case of namespaces.

The rospy Subscriber tutorial shows how to create a subscriber for a topic:

rospy.Subscriber(<topic_name>, <msg_type>, <callback_function>)

A quick search shows the topic of interest for you might be /darknet_ros_3d/bounding_boxes? If that's so, import the corresponding message type from gb_visual_detection_3d_msgs:

from gb_visual_detection_3d_msgs.msg import BoundingBoxes3d
$\endgroup$
1
  • $\begingroup$ Thanks for your response @MorganStark47. Here, pose_ee takes the constant values (as shown above in main.py file). Can you please guide me that how can I add the subscriber code to the main.py file and then how to store the continuously coming messages (in the form of x, y, z coordinates) in pose_ee? $\endgroup$
    – ahmed ali
    Feb 15 at 13:34

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.