how to combine coordinate program and auto docking program
hi, i have problem, the coordinate program which makes the robot go to specific coordinate already done but now i need to add auto docking program in it. So the robot will automatically perform auto docking when it detects its battery is low. How to make it? below is my program for coordinate
import rospy import actionlib from movebasemsgs.msg import MoveBaseAction, MoveBaseGoal from math import radians, degrees from actionlibmsgs.msg import * from geometrymsgs.msg import Point
class map_navigation():
def choose(self):
choice='q'
rospy.loginfo("|-------------------------------|")
rospy.loginfo("|PRESSE A KEY:")
rospy.loginfo("|'0': Tempat Asal ")
rospy.loginfo("|'1': Table 1 ")
rospy.loginfo("|'2': Table 2 ")
rospy.loginfo("|'3': Table 3 ")
rospy.loginfo("|'q': Quit ")
rospy.loginfo("|-------------------------------|")
rospy.loginfo("|WHERE TO GO?")
choice = input()
return choice
def init(self):
# declare the coordinates of interest
self.xAsal = 1.63
self.yAsal = 2.15
self.xTable1 = 2.41
self.yTable1 = -0.621
self.xTable2 = 1.92
self.yTable2 = -2.62
self.xTable3 = 2.02
self.yTable3 = 0.907
self.goalReached = False
# initiliaze
rospy.init_node('map_navigation', anonymous=False)
choice = self.choose()
if (choice == 0):
self.goalReached = self.moveToGoal(self.xAsal, self.yAsal)
elif (choice == 1):
self.goalReached = self.moveToGoal(self.xTable1, self.yTable1)
elif (choice == 2):
self.goalReached = self.moveToGoal(self.xTable2, self.yTable2)
elif (choice == 3):
self.goalReached = self.moveToGoal(self.xTable3, self.yTable3)
if (choice!='q'):
if (self.goalReached):
rospy.loginfo("Congratulations!")
#rospy.spin()
else:
rospy.loginfo("Hard Luck!")
while choice != 'q':
choice = self.choose()
if (choice == 0):
self.goalReached = self.moveToGoal(self.xAsal, self.yAsal)
elif (choice == 1):
self.goalReached = self.moveToGoal(self.xTable1, self.yTable1)
elif (choice == 2):
self.goalReached = self.moveToGoal(self.xTable2, self.yTable2)
elif (choice == 3):
self.goalReached = self.moveToGoal(self.xTable3, self.yTable3)
if (choice!='q'):
if (self.goalReached):
rospy.loginfo("Congratulations!")
#rospy.spin()
else:
rospy.loginfo("Hard Luck!")
def shutdown(self): # stop turtlebot rospy.loginfo("Quit program") rospy.sleep()
def moveToGoal(self,xGoal,yGoal):
#define a client for 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(10.0))):
rospy.loginfo("Waiting for the move_base 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(xGoal,yGoal,0)
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0
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 the destination")
return True
else:
rospy.loginfo("The robot failed to reach the destination")
return False
if name == 'main': try:
rospy.loginfo("You have reached the destination")
map_navigation()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("map_navigation node terminated.")
Asked by ejat on 2017-04-15 04:30:18 UTC
Comments
any answers?
Asked by ejat on 2017-04-15 10:55:17 UTC