Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#! /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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment