code.py

#! /usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)

pose_goal = rospy.subscriber('/door_handle_detection/pose_handle',geometry_msgs.msg.PoseStamped,queue_size=20)
#pose_goal.orientation.w = 0.0
#pose_goal.position.x = 0.4
#pose_goal.position.y = 0.3
#pose_goal.position.z = 0.4
group.set_pose_target(pose_goal)

group.go(wait=True)

rospy.sleep(5)

moveit_commander.roscpp_shutdown()