How to change orientation for navigation
How can I change the value goal.target_pose.pose.orientation.w = 1.0 to not be constant. I would like to assign it like this: self.xQR1 = -1.07449212074 self.yQR1 = 0.59228172302
self.wQR1 = ......
original code
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from math import radians, degrees
from actionlib_msgs.msg import *
from geometry_msgs.msg import Point
class map_navigation():
def choose(self):
choice='q'
rospy.loginfo("|-------------------------------|")
rospy.loginfo("|PRESSE THE KEY:")
rospy.loginfo("|'0': Start ")
rospy.loginfo("|'q': Quit ")
rospy.loginfo("|-------------------------------|")
rospy.loginfo("|WHERE TO GO?")
choice = input()
return choice
def __init__(self):
# declare the coordinates of interest
self.xQR1 = -1.07449212074
self.yQR1 = 0.59228172302
self.xQR2 = -1.8156080246
self.yQR2 = 10.0032901764
self.xQR3 = 30.44
self.yQR3 = 12.50
self.xQR4 = 35.20
self.yQR4 = 13.50
self.goalReached = False
# initiliaze
rospy.init_node('map_navigation', anonymous=False)
choice = self.choose()
if (choice == 0):
self.goalReached = self.moveToGoal(self.xQR1, self.yQR1)
QR1 = True
elif (QR1 == True):
self.goalReached = self.moveToGoal(self.xQR2, self.yQR2)
QR2 = True;
elif (QR2 == True):
self.goalReached = self.moveToGoal(self.xOffice2, self.yOffice2)
elif (choice == 3):
self.goalReached = self.moveToGoal(self.xOffice3, self.yOffice3)
if (choice!='q'):
if (self.goalReached):
rospy.loginfo("Congratulations!")
#rospy.spin()
while choice != 'q':
choice = self.choose()
if (choice == 0):
self.goalReached = self.moveToGoal(self.xQR1, self.yQR1)
elif (choice == 1):
self.goalReached = self.moveToGoal(self.xOffice1, self.yOffice1)
elif (choice == 2):
self.goalReached = self.moveToGoal(self.xOffice2, self.yOffice2)
elif (choice == 3):
self.goalReached = self.moveToGoal(self.xOffice3, self.yOffice3)
if (choice!='q'):
if (self.goalReached):
rospy.loginfo("Congratulations!")
#rospy.spin()
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(5.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.")