Issue combining subscribing with a node and calling a service in python

asked 2019-08-15 16:56:12 -0600

Tawfiq Chowdhury gravatar image

updated 2019-09-03 18:12:08 -0600

I am having an issue combining subscription to a topic and calling a service in python. I have a project where I need to subscribe to a topic and then use the topic data to make a service call. Subscribing to a topic requires a node to be initialized but the service call does not require it as shown in the tutorial. So, what happens is if I remove the node, the subscribing functionality does not work but the service call does, if I keep the node, the subscribing functionality does work but the service call does not. However, I need to combine both of them. Here is my code in python:

#!/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 *


def Pose_Send_Client():

    # This subscibes to a topic once and then stops 
    msg=rospy.wait_for_message('/sd/Pose', PoseStamped)
    print("Subscribed",msg.pose)

    print "============ Waiting for RVIZ..."
    #rospy.sleep(10)

    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.w
    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(pose_source.pose.position.x, pose_source.pose.position.y, pose_source.pose.position.z, pose_source.pose.orientation.x, pose_source.pose.orientation.y, pose_source.pose.orientation.z, pose_source.pose.orientation.w)
    except rospy.ServiceException, e:
      print "Service call failed: %s"%e



if __name__=='__main__':
    Pose_Send_Client()
edit retag flag offensive close merge delete

Comments

does not work

What does that mean? Can you please be specific

jayess gravatar image jayess  ( 2019-08-15 16:58:12 -0600 )edit

@jayess So, I can print the subscribed message, if it does not print, it means it is not subscribing, also I can check the service call by calling it, if I do not receive any output from the server, it means service call is not working.

Tawfiq Chowdhury gravatar image Tawfiq Chowdhury  ( 2019-08-15 17:11:02 -0600 )edit