def go_to_absolute(self, xyt_position, close_loop=True, smooth=False):
"""
Moves the robot to the robot to given goal state in
the world frame using proportional control.
:param xyt_position: The goal state of the form
(x,y,t) in the world (map) frame.
:param close_loop: When set to "True", ensures that
controler is operating in open loop by
taking account of odometry.
:param smooth: When set to "True", ensures that the
motion leading to the goal is a smooth one.
:type xyt_position: list or np.ndarray
:type close_loop: bool
:type smooth: bool
"""
assert (not smooth), "Proportional controller \
cannot generate smooth motion"
assert (close_loop), "Proportional controller \
cannot work in open loop"
pose_stamped = build_pose_msg(xyt_position[0], xyt_position[1],
xyt_position[2], self.MAP_FRAME)
base_pose = self._transform_listener.transformPose(
self.BASE_FRAME, pose_stamped)
return self.goto(self._get_xyt(base_pose))
def _send_action_goal(self, x, y, theta, frame):
"""A function to send the goal state to the move_base action server """
goal = MoveBaseGoal()
goal.target_pose = build_pose_msg(x, y, theta, frame)
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Waiting for the server")
self.move_base_sac.wait_for_server()
rospy.loginfo("Sending the goal")
self.move_base_sac.send_goal(goal)
rospy.sleep(0.1)
rospy.loginfo("Waiting for the Result")
start_time = time.time()
while True:
if time.time() - start_time > 10:
break
assert (
self.execution_status is not 4), \
"move_base failed to find a valid plan to goal"
if self.execution_status is 3:
rospy.loginfo("Base reached the goal state")
return
if self.base_state.should_stop:
rospy.loginfo(
"Base asked to stop. Cancelling goal sent to move_base.")
self.cancel_goal()
return
def _send_action_goal(self, x, y, theta, frame):
"""A function to send the goal state to the move_base action server """
goal = MoveBaseGoal()
goal.target_pose = build_pose_msg(x, y, theta, frame)
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Waiting for the server")
self.move_base_sac.wait_for_server()
rospy.loginfo("Sending the goal")
self.move_base_sac.send_goal(goal)
rospy.sleep(0.1)
rospy.loginfo("Waiting for the Result")
while True:
if self._as.is_preempt_requested():
rospy.loginfo("Preempted the Movebase execution")
self.cancel_goal()
return False
if self.execution_status is 4:
rospy.loginfo("move_base failed to find a valid plan to goal")
return False
if self.execution_status is 3:
rospy.loginfo("Base reached the goal state")
return True
if self.base_state.should_stop:
rospy.loginfo("Base asked to stop. Cancelling goal sent to move_base.")
self.cancel_goal()
return False