In the final exam in ROS Navigation, I got the error: “Service NOT responded with an OK”. Everything else works well. I tried troubleshooting but could not figure it out. Can i know what went wrong?
The main launch file associated with this is:
goal_service_server.launch
<launch>
<!-- Load map server, amcl, path planning -->
<include file="$(find navigation_exam)/launch/path_planning.launch" />
<node pkg="navigation_exam" name="pose_service_node" type="goal_service_server.py" output="screen">
<!-- <rosparam file="$(find navigation_exam)/params/points.yaml" command="load"/> -->
</node>
</launch>
The service file is as follows:
SendPosition.srv
string input_msg
---
string output_msg
The python file being used:
goal_service_server.py
#! /usr/bin/env python
import rospy
from navigation_exam.srv import SendPosition, SendPositionResponse # you import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import PoseStamped
def my_callback(request):
print("Service request received=",request)
if request.input_msg=="point1":
pub.publish(goal1)
elif request.input_msg=="point2":
pub.publish(goal2)
Resp_obj.output_msg="OK"
return Resp_obj
rospy.init_node('service_server')
Resp_obj=SendPositionResponse()
# creates a goal to send to the action server
goal1 = PoseStamped()
goal2= PoseStamped()
print("goal object created")
goal1.header.frame_id="map"
goal1.pose.position.x=rospy.get_param('/move_base/point1/position/x')
goal1.pose.position.y=rospy.get_param('/move_base/point1/position/y')
goal1.pose.position.z=rospy.get_param('/move_base/point1/position/z')
goal1.pose.orientation.x=rospy.get_param('/move_base/point1/orientation/x')
goal1.pose.orientation.y=rospy.get_param('/move_base/point1/orientation/y')
goal1.pose.orientation.z=rospy.get_param('/move_base/point1/orientation/z')
goal1.pose.orientation.w=rospy.get_param('/move_base/point1/orientation/w')
print("Goal 1 data input complete")
goal2.header.frame_id="map"
goal2.pose.position.x=rospy.get_param('/move_base/point2/position/x')
goal2.pose.position.y=rospy.get_param('/move_base/point2/position/y')
goal2.pose.position.z=rospy.get_param('/move_base/point2/position/z')
goal2.pose.orientation.x=rospy.get_param('/move_base/point2/orientation/x')
goal2.pose.orientation.y=rospy.get_param('/move_base/point2/orientation/y')
goal2.pose.orientation.z=rospy.get_param('/move_base/point2/orientation/z')
goal2.pose.orientation.w=rospy.get_param('/move_base/point2/orientation/w')
print("Goal 2 data input complete")
print("Starting publisher")
pub=rospy.Publisher('/move_base_simple/goal',PoseStamped,queue_size=1)
while pub.get_num_connections()<1:
pass
print("Publisher ready")
print("starting service")
my_service = rospy.Service('/send_pose_service', SendPosition , my_callback) # create the Service called my_service with the defined callback
print("Service started")
rospy.spin() # maintain the service open.
