def _callback_nav_service(req, x, y, yaw):
# define a client to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# wait for the action server to come up
while(not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.logwarn("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
goal.target_pose.pose.position = Point(x ,y, 0)
orientation = tf.transformations.quaternion_from_euler(0, 0, yaw)
goal.target_pose.pose.orientation.x = orientation[0]
goal.target_pose.pose.orientation.y = orientation[1]
goal.target_pose.pose.orientation.z = orientation[2]
goal.target_pose.pose.orientation.w = orientation[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
if(ac.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo("You have reached goal")
ser_messageResponse(True)
else:
rospy.logerr("The robot failed to reach goal")
ser_messageResponse(False)
def _callback_waypoint_nav(req, x, y, yaw, service_name):
waypoint = rospy.get_param("/nav_waypoint_services/"+service_name+"/waypoint_list")
for ii in range (len(waypoint)):
# define a client to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# wait for the action server to come up
while(not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.logwarn("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
goal.target_pose.pose.position = Point(x[ii] ,y[ii], 0)
orientation = tf.transformations.quaternion_from_euler(0, 0, yaw[ii])
goal.target_pose.pose.orientation.x = orientation[0]
goal.target_pose.pose.orientation.y = orientation[1]
goal.target_pose.pose.orientation.z = orientation[2]
goal.target_pose.pose.orientation.w = orientation[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
if(ac.get_state() == GoalStatus.SUCCEEDED):
rospy.loginfo("Reached waypoint "+waypoint[ii])
ser_messageResponse(True)
else:
rospy.logwarn("Failed reaching waypoint "+waypoint[ii])
ser_messageResponse(False)
def _callback_navigate_room_1(req):
# define a client to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# wait for the action server to come up
while (not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.logwarn("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
goal.target_pose.pose.position = Point(
7.212, 4.388, 0) # (-13.759, -3.9, 0) (5.762 ,4.326, 0)
orientation = tf.transformations.quaternion_from_euler(
0, 0, 0) # (0, 0, 3.111) (0, 0, 1.39)
goal.target_pose.pose.orientation.x = orientation[0]
goal.target_pose.pose.orientation.y = orientation[1]
goal.target_pose.pose.orientation.z = orientation[2]
goal.target_pose.pose.orientation.w = orientation[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
time.sleep(4)
feedback_pub = rospy.Publisher("feedback_for_nav", String, queue_size=1)
if (ac.get_state() == GoalStatus.SUCCEEDED):
rospy.logerr("You have reached the open area")
ser_messageResponse(True)
msg = String()
msg.data = "success"
for ii in range(7):
feedback_pub.publish(msg)
time.sleep(1)
else:
rospy.logerr("The robot failed to reach the open area")
ser_messageResponse(False)
msg = String()
msg.data = "failure"
for ii in range(7):
feedback_pub.publish(msg)
time.sleep(1)
