Service call failed: transport error completing service call: unable to receive data from sender, check sender's logs for details
I am trying to subscribe to a pose using a python script then from that script, I am trying to send a pose using service call which will be received later by a C++ server to perform a pick and place.
Here is the python code:
#!/usr/bin/env python
import sys
import copy
import rospy
import roscpp
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
import math
import time
from pr2_package.srv import *
from numpy.linalg import inv
from std_msgs.msg import String
import math
def Pose_Send_Client():
rospy.init_node('listener', anonymous=True)
msg=rospy.wait_for_message('chatter', PoseStamped)
print("Subscribed",msg.pose)
print "============ Waiting for RVIZ..."
rospy.sleep(10)
print "============ Generating plan 1"
load = np.loadtxt('/home/tawfiq/pr2_ws/pose.txt')
quaternion3 = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion3) #, axes='szyx'
roll=euler[0]
pitch=euler[1]
yaw=euler[2]
C00=math.cos(yaw)*math.cos(pitch)
C01=math.cos(yaw)*math.sin(pitch)*math.sin(roll) - math.sin(yaw)*math.cos(roll)
C02=math.cos(yaw)*math.sin(pitch)*math.cos(roll) + math.sin(yaw)*math.sin(roll)
C03=msg.pose.position.x
C10=math.sin(yaw)*math.cos(pitch)
C11=math.sin(yaw)*math.sin(pitch)*math.sin(roll) + math.cos(yaw)*math.cos(roll)
C12=math.sin(yaw)*math.sin(pitch)*math.cos(roll) -math.cos(yaw)*math.sin(roll)
C13=msg.pose.position.y
C20= -math.sin(pitch)
C21=math.cos(pitch)*math.sin(roll)
C22=math.cos(pitch)*math.cos(roll)
C23=msg.pose.position.z
C30=0
C31=0
C32=0
C33=1
object_new_mat=np.array([[C00, C01, C02, C03],[C10, C11, C12, C13],[C20, C21, C22, C23],[C30, C31, C32, C33]])
print("object_new_mat",object_new_mat)
transformation_mat = np.loadtxt('/home/tawfiq/pr2_ws_test/transform.txt')
Final_pose=np.dot(object_new_mat, transformation_mat)
roll=math.atan2(float(Final_pose[2][1]),float(Final_pose[2][2]))
pitch=math.atan2(-Final_pose[2][0],float(math.sqrt(Final_pose[2][1]**2 + Final_pose[2][2]**2)))
yaw=math.atan2(float(Final_pose[1][0]),float(Final_pose[0][0]))
print("Mine", roll, pitch, yaw)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
pose_source = PoseStamped()
pose_source.header.frame_id = "odom_combined"
pose_source.pose.orientation.x = msg.pose.orientation.x
pose_source.pose.orientation.y = msg.pose.orientation.y
pose_source.pose.orientation.z = msg.pose.orientation.z
pose_source.pose.orientation.w = msg.pose.orientation.z
pose_source.pose.position.x = msg.pose.position.x
pose_source.pose.position.y = msg.pose.position.y
pose_source.pose.position.z = msg.pose.position.z
rospy.wait_for_service('Pose_Send')
try:
Pose_Send = rospy.ServiceProxy('Pose_Send', PoseSend)
grasping = Pose_Send(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
if __name__=='__main__':
Pose_Send_Client()
Here is ...