Revision history [back]

I have also implemented the other thing that I need, so I am pasting my full code here:

#!/usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from geometry_msgs.msg import PoseStamped
import tf
import roslib
import traceback
from std_msgs.msg import String
import numpy as np
import tf2_ros
import tf2_geometry_msgs

def listener_new():
 rospy.init_node('listener_new', anonymous=True)
 msg=rospy.wait_for_message('/sd/Pose', PoseStamped)
 print(msg.pose)
 project(msg.pose)


def project(msg):
  #x = np.array([[msg.position.z, msg.position.z], [msg.position.y, msg.position.y]])
  #np.savetxt('text.txt', x)
  ## Initialize moveit commander
  print "============ Starting setup"
  moveit_commander.roscpp_initialize(sys.argv)
  #rospy.init_node('project1', anonymous=True)

  ## Instantiate a RobotCommander object.  This object is an interface to
  ## the robot as a whole.
  robot = moveit_commander.RobotCommander()

  ## Instantiate a PlanningSceneInterface object.  This object is an interface
  ## to the world surrounding the robot.
  scene = moveit_commander.PlanningSceneInterface()

  ## Instantiate a MoveGroupCommander object.  This object is an interface
  ## to one group of joints.  In this case the group is the joints in the left
  ## arm.  This interface can be used to plan and execute motions on the left
  ## arm.
  group = moveit_commander.MoveGroupCommander("right_arm")


  ## We create this DisplayTrajectory publisher which is used below to publish
  ## trajectories for RVIZ to visualize.
  display_trajectory_publisher = rospy.Publisher(
                                      '/move_group/display_planned_path',
                                      moveit_msgs.msg.DisplayTrajectory)

  ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
  print "============ Waiting for RVIZ..."
  rospy.sleep(10)


  print "============ Generating plan 1"

  p1 = geometry_msgs.msg.PoseStamped()
  p1.header.frame_id = "head_plate_frame"
  p1.pose.position.x = msg.position.x
  p1.pose.position.y = msg.position.y
  p1.pose.position.z = msg.position.z
  p1.pose.orientation.w = msg.orientation.x
  p1.pose.orientation.w = msg.orientation.x
  p1.pose.orientation.w = msg.orientation.x
  p1.pose.orientation.w = msg.orientation.x

  tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
  tf_listener = tf2_ros.TransformListener(tf_buffer)

  transform = tf_buffer.lookup_transform("base_link",
                                       p1.header.frame_id, #source frame
                                       rospy.Time(0), #get the tf at first available time
                                       rospy.Duration(1.0))
  pose_transformed = tf2_geometry_msgs.do_transform_pose(p1, transform)
  print("pose_transformed",pose_transformed.pose)


  pose_source = geometry_msgs.msg.Pose()
  pose_source.orientation.x = 0.4
  pose_source.orientation.y = 0.3
  pose_source.orientation.z = 0.2
  pose_source.orientation.w = 0.2
  pose_source.position.x = 0.74
  pose_source.position.y = -0.07
  pose_source.position.z = 1.1




  group.set_pose_target(pose_source)

  ## Now, we call the planner to compute the plan
  ## and visualize it if successful
  ## Note that we are just planning, not asking move_group 
  ## to actually move the robot
  plan1 = group.plan()

  print "============ Waiting while RVIZ displays plan1..."
  rospy.sleep(5)

  # Uncomment below line when working with a real robot

  # group.go(wait=True)


  ## First, we will clear the pose target we had just set.

  group.clear_pose_targets()

  ## When finished shut down moveit_commander.
  moveit_commander.roscpp_shutdown()

  print "============ STOPPING"


if __name__=='__main__':
  try:
    listener_new()
  except rospy.ROSInterruptException:
    pass