ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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